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I.  INTRODUCTION 

In  motion  execution  of  a  robot  arm,  one  basic  problem  is  to  design  a  control 
system.  This  problem  is  complicated  because  the  dynamic  motion  of  a  manipulator  is 
strongly  influenced  by  mechanical  design  and  physical  properties  of  the  manipulator,  as 
well  as  environmental  effects.  Coupling  inertia,  joint  friction,  coriolis  forces,  centripetal 
forces,  actuator  dynamics  and  gravity  effects  are  considered  among  these  factors. 
These  factors  produce  an  interactive  nonlinear  dynamic  system.  Under  these 
circumstances  a  robust  and  flexible  control  system  is  needed. 

The  curve  following  scheme  is  used  in  all  high  performance  disk  files  as  part  of 
the  read/write  head  positioning  servo.  It  has  been  shown  [Ref  1]  that  the  entire  loop 
can  be  modelled  in  a  computer  or  microprocessor  and  the  model  can  be  used  to  drive  a 
motor.  Algorithms  were  developed  to  adapt  the  computer  model  for  small  parameter 
discrepancies  and  to  update  the  states  in  the  computer.  In  the  robot  arm  there  will  be 
large  parameter  variations  in  mass  and  moment  of  inertia  due  to  loading  and  the 
nonlinear  dynamics  q[  the  manipulator. 

The  purpose  of  this  thesis  is  to  investigate  the  possibility  of  using  the  Curve 
Following  Velocity  Loop  Servo  to  obtain  near  minimum  time  positioning  of  a  robot 
manipulator.  In  this  study,  a  two  hnk  robot  manipulator  with  two  degrees  of  freedom 
is  chosen  as  the  simulation  model.  The  same  control  scheme  is  used  to  drive  both 
motors. 

Figure  1.1  is  a  simplified  block  diagram  of  the  control  scheme  to  be  investigated. 
The  second  order  model  is  a  simple  approximation  of  the  actuator.  A  Velocity 
feedback  loop  is  used  in  the  adaptive  model  only.  The  "Adaptive  Algorithm"  uses 
Actuator  position  output  to  calculate  the  Actuator  velocity,  and  updates  the  second 
order  model  gain  constant,  position  and  velocity  at  certain  time  intervals.  Therefore 
the  requirement  for  a  tachometer  is  eliminated.  The  "Curve"  approximates  the 
deceleration  curve  of  an  ideal  motor.  It  is  chosen  as  a  parabola.  The  "Environment" 
represents  Gravitational  torques,  Centripetal  forces,  Coriohs  forces  and  Inertia  loading. 
The  "Actuator"  includes  the  servo  motor  and  load.  Both  the  second  order  model  and 
the  actuator  are  driven  by  the  same  velocity  error  input. 
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Figure  1.1     Simplified  Block  Diagram  of  the  System. 

Development  of  the  computer  simulation  model  is  presented  in  Chapter  11. 
Chapter  III  discusses  the  modelling  of  the  robot  arm.  The  computer  adaptive  model  is 
developed  in  Chapter  IV.  Simulation  studies  are  planned  in  Chapter  V.  Simulations 
under  difTerent  conditions  for  a  Voltage  Source  Drive  System  are  studied  in  Chapter 
VI.  Chapter  VII  discusses  the  development  of  a  Current  Source  Drive  System  and 
presents  simulation  studies  under  different  conditions.  Conclusions  and  further  study 
areas  are  presented  in  Chapter  Vlll.  DSL  simulation  programs  and  derivation  of 
mathematical  model  for  robot  arm  are  given  in  Appendi.xes  A  —  F. 
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II.  DEVELOPMENT  OF  THE  COMPUTER  SIMULATION  MODEL 

A.       INTRODUCTION 

It  can  be  shown  [Ref.  2]  that  the  equivalent  transfer  function  of  the  servo  motor 
is 


0(s)  I'K,. 

77-    =    -^ h—, X  {eqn2.1) 

V{s) 


where  parameters  0.  V,  K^.,  K^.  J,  R.  and  L  are  defined  in  Chapter  VII.  The  arm  of  a 
robot  is  a  large  inertia,  and  when  this  is  added  to  the  motor  inertia  the  mechanical  pole 
of  the  motor  becomes  smaller.  Since  R,  L  is  normally  a  large  number,  the  electrical 
pole  of  the  motor  may  be  neglected  and  the  transfer  function  of  the  robot  arm  and 
motor  becomes  approximately 

6(5)     ~     K^ 

7—    =    -f-  (eqn2.2) 

V(s)  s^ 

Figure  2.1  shows  the  second  order  model  with  a  curve  following  velocity  loop. 
The  Curve  used  is  the  deceleration  curve  for  the  idealized  motor.  Bang-Bang  control  is 
chosen  to  dri\'e  the  system  with  full  power.   [Ref  3] 

For  a  given  step  position  command,  the  error  signal  (E)  will  produce  a  velocity 
command  input.  This  signal  saturates  the  amplifier  and  drives  the  system  into  full 
acceleration.  When  the  commanded  velocity  signal  (X)  becomes  equal  to  the  velocity 
feedback  signal  (KP)  the  system  changes  into  curve  following  mode  and  follows  the 
curve  until  the  desired  position  is  reached. 

B.       OBTAINING  THE  CURVE  AND  MODEL  PARAMETERS 

Selection  of  the  curve  must  be  done  in  such  a  way  that  the  system  will  be  able  to 
follow  the  curve  under  different  loading  conditions.  Since  a  parabolic  curve 
approximates  the  deceleration  curve  of  an  ideal  motor,  it  was  chosen  as  the  curve  to  be 
followed. 
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Figure  2.1     Block.  Diagram  of  the  Model. 

In  the  robot  arm,  simulation  studies  show  that  the  curve  must  be  shaped  due  to 
the  mechanical  design  of  the  arm.  actuator  parameters  and  gravity  effects.  Therefore 
the  gain  constant  (Kl)  is  used  to  reshape  the  curve.  In  our  case  Kl  was  set  to  a  value 
of  0.6,  It  has  been  shown  [Ref  1]  that  the  equation  of  the  curve  was  derived  from  the 
idealized  motor  equations  as  follows: 


C  =  K^  Vsat 


(eqn  2.3) 


C  =  I  C  dt  =  Kj^  Vsat  t  +  C(0),  (C(0)  =  0) 


C  =  J"  C  dt  =   l,2{Kj^  Vsat  t")  +  C(0) 


(eqn  2.4) 
(eqn  2.5) 


When  we  solve  Equation  2.4  for  t  and  substitude  into  Equation  2.5. 


C  = 


C^ 


2  Kj^  Vsat 


(eqn  2.6) 


For  deceleration  from  initial  conditions  with  the  input  R  =  0, 
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C  =    -  E  (eqn  2.7) 


C  =    -E  (eqn  2.8) 

finally  when  we  substitude  Equation  2.7  and  2.8  into  Equation  2.6  we  come  up  with: 

•  X  =  V^Kj^Vsat  -sfE  =  commanded  velocity 

The  second  order  model  gain  constant  (K^^)  is  determined  by  the  actuator 
parameters  and  the  elTective  inertia  seen  by  the  actuator.  The  values  of  K^  were  given 
below  (to  be  derived  in  Chapter  IV. 

•  K^i  =  0.17  rad.volt 

•  K^T  =  4.0  rad,  volt 

The  saturation  limits  of  the  amplifiers  are  determined  by  available  servo  motor 
parameters,  mechanical  design  of  the  arm.  working  conditions  and  curve  constant  (Kl). 
In  this  study,  the  same  servo  motor  is  used  for  both  actuators,  whether  or  not  gravity 
loading  exists.    The  values  of  Vsat  are: 

•  Vsatl  =   150  volts,  Vsat2  =  50  volts  (gravity-free  environment) 

•  Vsatl  =  300  volts.  Vsat2  =   150  volts  (with  gravity  loading) 

The  gain  parameter  (K2)  o(  the  saturation  amplifier  is  chosen  to  saturate  the 
amplifier  for  small  signals  so  that  it  acts  as  a  switch  to  drive  the  system  with  full 
forward  and  reverse  drive  signals  in  the  curve  following  mode.  Therefore  K2  was  set  to 
the  value  of  10.000  to  guarantee  the  saturation  of  the  amplifier. 

The  gain  of  the  velocity  feedback  channel  (K)  has  been  given  [Ref  1]  K  =  Kl. 
In  hardware  realization  of  the  system  the  value  of  K  can  be  adjusted  to  compare  the 
commanded  velocity  signal  and  the  velocity  feedback  signal.  For  our  purpose 
simulation  studies  show  the  best  curve  following  is  achieved  for  K  =   1. 

C.       SIMULATION  STUDIES  OF  THE  MODEL 

The  model  was  simulated  using  DSL  VS.  The  DSL  simulation  program  is  listed 
in  Appendi.x  A.  Vsat  and  K^  were  set  to  150  volts,  4.0  rad.volt  respectively  for  these 
studies.  Figure  2.2  shows  phase  plane  trajectories  (Angular  velocity  P  versus  angular 
position  P)  for  a  step  position  command  of  1  radian.  The  figure  shows  that  the  model 
angular  velocity  increases  with  constant  acceleration  until  the  curve  is  reached.  Then 
the  voltage  applied  to  the  model  reverses  and  the  model  velocity  follows  the  curve  until 
the  desired  angular  position  is  reached.  The  step  response  o(  the  model  is  shown  in 
Figure  2.3. 
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Figure  2.2     Phase  Plane  trajectories  of  the  Model. 


17 


2.ac- 

1.7S- 

1.50- 

1.2S- 

0.75- 

/^ 

0.5^ 

/ 

MM* 

0.25- 

/ 

/ 

a_ 

0.0Q_ 

y 

0          20 

40 

80          eC        100        1M        140 
Tir«    (10-»S£Ct>t^) 

1S0 

180 

WO 

STEP  RESPONSE 
P  VS   TIHE 

Figure  2.3     Step  Response  of  the  Model. 


IS 


III.  MODELLING  THE  ROBOT  ARM 

A.  INTRODUCTION 

In  this  chapter,  a  mathematical  model  of  a  two-degrees-of-freedom  planar  robot 
arm  is  developed  using  lagrangian  mechanics.  The  dynamics  equations  relate  torques 
and  forces  to  positions,  velocities  and  accelerations.  We  need  dynamics  equations  to 
get  the  equations  of  motion  of  the  manipulator.  We  need  to  know  the  relationship 
between  torque  and  acceleration  at  a  joint,  and  the  relationship  between  torque  at  one 
joint  and  acceleration  at  other  joints.  We  also  need  to  know  effects  of  the  gravity 
torques  and  velocity  dependent  torques.  Velocity  dependent  torques  are  typically  small 
compared  to  other  system  torques.  They  are  important  when  the  manipulator  is 
moving  at  high  speed. 

B.  MODEL  DEVELOPMENT 

The  two-degrees-of-freedom  planar  robot  arm  considered  is  illustrated  in  Figure 
3.1.  The  motion  of  the  robot  arm  is  constrained  to  the  xy  plane.  Both  jomts  are 
rotar\'  joints.  There  are  two  links,  connecting  the  two  joints  and  a  terminal  grasping 
device  at  J0INT2  with  or  without  load.  It  is  assumed  that  the  links  are  massless  and 
two  equivalent  masses  are  lumped  at  the  end  of  LINKl  and  LINK2.  The  lengths  of 
LINKl  and  LINK2  are  denoted  d^  d.,  and  two  masses  are  denoted  m^,  m2 
respectively.   T,,  T,  represents  the  external  driving  torques  to  the  joints. 

C.  DERIVATION  OF  EQUATION  OF  MOTION 

The  lagrangian  L  is  defined  as  the  difference  between  the  kinetic  energy  K  and 
the  potential  energy  V  of  the  system.  L  =  K  -  V 

Equation  3.1  defines  the  Lagrange's  Equation 
d     (dL    \       dL 


dt    \dq-^  j       cq^ 

where 


=  F-      i=  l,...,n  (eqn  3.1) 


q-    =  generalized  coordinates 

q-    =  corresponding  velocity 

F;    =  the  corresponding  force  or  torque 
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Figure  3.1     Point  Mass  Reoresentation  of  a  Two-Dearees-Of-Freedom 

ptanar  Robot  Arm. 

L  =  Langrangian  of  the  system 
n  =  number  of  degrees  of  freedom 
To  apply  equation  3.1  to  the  robot  arm,  the  rotation  angles  9,  and  0-,  are  chosen 
as  the  generalized  coordinates.  The  generalized  forces  F,,  F-,  would  be  the  total 
torques  about  the  axis  of  rotation  ofQ^  and  0.,  The  detailed  derivation  is  presented  in 
Appendi.x  B.  The  following  pair  of  second-order  nonlinear  diflerential  equations  has 
been  derived  and  will  be  used  as  the  basic  mathematical  model  for  this  thesis. 


(^ll-^Vl)®l  =  "^1  "  (^12^2  ^  ^122^2" 

+  D]i20]0')  ■*"  ''-^121^1^2  "^^l) 


(eqn  3.2) 


k  '> 


(^22  ^  ■^m2)®2  =  "^2  ~  ^^12^1  ^  ^2119^  ^  ^2) 


(eqn  3.3) 
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where 

Dji  =  (  mj  +  m2  )d^'^  +  m2d2    +  2m2d|d2cos02 

Dp  =  mjd^"-  +  m'>djd2Cos62 

D->-,  =  m->d7"- 

Dp2^    ~  m-)d|d2sin02 

^112^    ~  m2d|d2sin02  =  Dpj 

^211=  ^2^1^2sii^Q2 

Gj    =  [(m| +  m->)d|Cos02  +  m2d2Cos(9j +02)]g 
G2    =  mod7cos(0i +02)g 
•^ml  ~  rnotor  inertia  for  Joint  1  motor 
Jj^-)  =  motor  inertia  for  Joint  2  motor 
nij    =  0.24S  oz  in  sec- 
m-)    =  0,041  oz  in  sec" 
m-)    =  0.082  OZ; in,  sec"   (with  load) 
d|     =15  inches 
do     =10  inches 
g       =  386.4  m  sec- 
Let  us  rewrite  Equations  3.2  and  3.3  in  the  form 

"^1  =  (^11  t  V^^'^'l  ^  ^12^*2  (^q^  3-^) 

^^112^1^2  ^  ^121^1^2 
+  D,oo0'^ 


122^2 


T2  =  (D22  +  Jm2)^2  ^  ^12'®*!  ^^^^  ^•^) 

+  ^211^1^ 
4-G2 

•In  these  equations  (D|j+Jj^|)  and  ^^^j'^^rn^^  ^^^  known  as  the  elTective 
inertia  at  joints  1  and  2.  Accelerations  at  these  joints  cause  torques  and  are  referred  to 
as  inertial  torques.  Dp  is  known  as  the  coupling  enertia  between  JOINT  1  and 
JOINT2.  Dp02  is  the  reaction  torque  actmg  on  JO  INT  1  induced  by  the  angular 
acceleration  of  J0INT2;  Di20i  is  the  reaction  torque  acting  on  J0INT2  induced  by 
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angular  acceleration  of  JOINTl.  Dp292"  ^^  ^^'^^  centripetal  force  acting  on  JOINTl 
generated  by  the  angular  velocity  of  J0INT2;  ^2\\^\  ^^  ^^^^  centripetal  force  acting 
on  J0INT2  due  to  angular  velocity  at  JOINTl.  ^[[i^i^j  ^  0^216162^^^'^^''°"°^^^ 
force  acting  on  JOINTl  generated  by  the  angular  velocities  of  both  JOINTl  and 
J0INT2.  G|  and  G2  represent  the  gravity  forces  on  JOINTl  and  J0INT2 
respectively.    [Ref  4] 
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IV.  THE  COMPUTER  ADAPTIVE  MODEL 

A.  INTRODUCTION 

The  second  order  model  of  Chapter  II  is  to  be  used  to  drive,  open  loop,  the  servo 
motor  of  one  joint  of  the  robot  arm.  The  servo  motor  has  to  follow  a  predetermined 
curve  until  a  desired  position  is  reached.  To  accomplish  this,  the  second  order  model 
states  and  gain  constant  have  to  be  adapted  in  such  a  way,  that  the  model  mimics  the 
position  and  velocity  and  has  a  gain  constant  equivalent  to  that  of  the  actual  servo 
motor.  In  this  chapter  we  will  first  discuss  the  selection  of  servo  motors  for  testing  of 
the  adaptive  systems  and  will  then  obtain  the  algorithm  to  update  the  second  order 
model  position,  velocity  and  gain  constant.  The  calculation  of  the  initial  value  of  the 
model  gain  constant  (K^^)  is  also  given  in  this  Chapter. 

B.  SELECTION  OF  THE  SERVO  MOTORS 

A  permanent  magnet  motor  drive  currently  used  in  industrial  robots  was  selected 
to  test  the  adaptive  system.    Parametric  data  for  the  motor  are  Usted  in  Table  l,[Ref  .^] 


TABLE  1 

PARAMETRIC  DATA  FOR  JOINT  SERVO  .MOTORS 

Torque  Constant  K.^. 

14.  4 

oz-in/amp 

Total  Inertia  J^^ 

0.  033 

oz-in-sec-/rad 

Damping  Coefficient 

Bm 

0. 04297 

oz-in-sec/rad 

Back  emf  Const.  K.^ 

0. 1012 

volts-sec/rad 

Armature  Inductance 

L 

100 

ji-henries 

Ave.  Terminal  Resis" 

tanc 

e  R 

0.  91 

ohms 

The  same  motor  drive  is  used  for  both  JOINT!  and  J0INT2  with  a  different 
power  supply.  To  calculate  the  transfer  functions  of  the  servo  motors,  we  need  to 
know  the  effective  joint  inertias: 

•      -^EFFl  "  ^11  "^  -^ml 
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Where  D,  i,  D^o.  Jj^],  ^^n^  are  given  in  Chapter  II.    When  we  substitute  parameters, 
we  come  up  with: 

•  J£ppi  =  81.425     oz.in.s-      (for  02  =  0°) 

•  ^HFF''  ^  '^■^^     oz.in.s" 

Substituting  efiective  inertias  calculated  above  and  parameters  given  in  Table  1  into 
Equation  2.1,  gives  us  the  transfer  functions  of  the  servo  motors. 


9.S8 

Gl(s)  =  radians/volts  {eqn4.1 

S(S/9100+1)(S,  0.019+1) 


9. 88 

G2(s)  =  radians/volts  (eqn  4.2) 

S(S  9100+ IKS  0.39+1) 


The  Open  Loop  Bode  plots  for  the  servo  motors  are  shown  in  Figures  4.1  and 
4.2.  From  the  Open  Loop  Bode  plots  it  can  be  observed  that  the  -  40dB;decade  slope 
crosses  the  0.0  dB  axis  at  C0j  =  0.412  rad;sec  and  (02=  2.0  rad,  sec  for  JOINTl  and 
J0INT2  servo  motors  respectively.  When  we  use  the  cOj  and  0)2  frequencies  as  the 
gain  crossover  frequencies  of  the  second-order  ideal  motors,  we  come  up  with: 

•  ^ml  "  '^■■^^  rad; volt 

•  K^2  "^  •^•0   rad;  volt 

Thus  the  gain  constants  K  |  and  ^^2  °^  ^^^  second-order  models  were  set 
initially  to  these  calculated  values  to  determine  the  cur\'e  derived  in  Chapter  II.  Open 
Looo  Bode  diagrams  of  the  K^/s~  ideal  motors  are  eiven  in  Figures  4.3  and  4.4. 

C.       OBTAINING  THE  ADAPTIVE  ALGORITHM 

Figure  4.5  shows  block  diagram  of  the  adaptive  system  for  a  joint  servo  drive. 
The  same  adaptive  scheme  is  used  to  drive  both  servo  motors  separately.  In  this 
diagram  Tt  refers  to  the  sum  of  disturbance  torques  such  as  Centripetal  Torques. 
Centrifugal  Torques,  Reaction  Torques  and  Gravitational  Torques.  The  saturation 
amplifier  output  signal  is  common  input  to  both  the  second-order  model  and  servo 
motor.  Output  of  the  servo  motor  is  measured  and  fed  into  the  Adaptive  Algorithm 
block.  Our  next  step  will  be  to  calculate  an  estimate  of  the  servo  motor  gain 
parameter  (K^x  and  the  velocity  of  the  servo  motor.  In  these  calculations,  two  criteria 
must  be  taken  into  consideration: 
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Figure  4.1     Open  Loop  Bode  Plot  of  the  JOINTI  Servo  Motor. 
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Figure  4.2     Open  Loop  Bode  Plot  of  the  J0INT2  Servo  Motor. 
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Figure  4.5     Block  Diagram  of  thie  Adaptive  Joint  Drive  System. 
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V.  PLANNING  THE  SIMULATION  STUDIES 

A  set  of  sample  motions  was  selected  to  serve  as  a  basis  for  simulation  studies  of 
the  system.  The  sample  motions  were  designed  to  remove  as  many  of  the 
nonlinearities  as  possible  and  to  investigate  the  system  behavior  for  possible 
overloading  conditions.  The  set  of  sample  motions  used  are  depicted  in  Figures  5.1,  5.2 
and  5.3. 

Simulation  studies  were  done  for  two  different  drive  systems.   These  were: 

•  Voltage  Source  Drive 

•  Current  Source  Drive 

Each  drive  system  was  tested  separately  for: 

•  Gravity- Free  Environment 

•  With  Gravity  Loading 

Position  Command  inputs  to  the  system  were: 

•  Step  Input 

o      Time  Varv'ing  Input 

In  this  thesis,  a  free  work  space  is  assumed  for  the  robot  arm.  To  find  the 
minimum  time  response  for  a  given  step  position  command,  a  step  input  was  applied  to 
the  servo  motors; 

•  At  the  same  time 

•  At  different  times 

All  simulation  studies  are  planned  for  both  loaded  and  unloaded  robot  arms. 
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•  .     The  calculations  must  be  accurate  enouah  to  allow  the  second-order  model 
states  to  approximate  the  trajectory'  of  the^servo  motor. 

•.     The  calculations  must  be  kept  simple  to  update  the  second-order  model  states 
and  gam  constant  in  minimum  time. 

There   are    several   means   of  doing   these   calculations.     For   our  purpose,   the 

method  described  in  [Ref.  1]  best  satisfies  the  criterion  given  above.    It  can  be  shown 

[Ref.  1]  that;  when  we  solve  Equation  2.5  for  K 

III, 

2P 

^^^       \  sat  t- 


For  discrete  time  intervals  and  letting  P  =  PM, 


2PM 
^m  ~  T-  (eqn  4.4) 


Where  T  is  the  sampling  period  and  N  is  the  number  of  sampling  intervals.  Equation 
4.4  is  vahd  only  for  constant  acceleration  of  the  servo  motor.  Therefore  the  value  of 
K^  is  updated  during  the  full  acceleration  of  the  servo  motor,  then  remains  unchanged 
for  the  curve  following  portion. 

The  estimation  of  the  angular  velocity  of  the  servo  motor  has  been  given  [Ref  1] 

P*M(N)  =  2[PM(N)  -  PM(N  -  1)],T  -  PM(N  -  1)  (eqn  4.5) 

PM(N  -  1)  =  [PM(N)  -  PM(N  -  2)]/2T  (eqn  4.6) 

Equation  4.5  requires  the  storage  of  the  last  sample  position  of  the  servo  motor 
[PM(N-  1)]  and  the  last  estimated  angular  velocity  [P\[(N-  1)].  The  stored  value  of 
PM(N— .  1)  can  be  recalculated  after  2  samples  of  position  are  taken.  If  the  second- 
order  model  switches  from  full  acceleration  to  curve  following  between  samples  of 
position.  Equation  4.5  is  not  self-correcting  until  two  position  samples  can  be  taken 
after  switching.  To  take  care  of  the  deficiency,  the  switching  time  is  detected  and  the 
present  value  of  the  second-order  model  velocity  is  stored  as  PM(N-  1)  to  be  used  in 
the  next  calculation. 
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FINAL   POSITION 
PM1=  1  radian 
PM2=  1  radian 


INITIAL 
PM1{0)= 
PM2(0)= 


POSITION 
0  radian 
0  radian 


Figure  5.1 


Sample  Move  #1. 
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INITIAL   POSITION 
PM1(0)=  1.5  radian 
PM2(0)=  0   radian 


FINAL  POSITION 
PM1=  0  radian 
PM2=-1.5  radian 


Figure  5.2     Sample  Move  #2. 
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INITIAL   POSITION 
PM1(0)=  0  radian 
PM2(0)=  0  radian 


FINAL  POSITION 
PM1=-1.5  radian 
PM2=  1.5  radian 


Figure  5.3     Sample  Move  #3. 
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VI.  THE  VOLTAGE  SOURCE  DRIVE  SYSTEM 

A.  INTRODUCTION 

The  computer  adaptive  system  with  voltage  source  drive  was  developed  in 
Chapter  IV.  Figure  4.5  illustrates  the  block  diagram  of  the  adaptive  system  with 
voltage  source  drive.  The  DSL.'VS  simulation  program  used  in  the  testing  of  the 
adaptive  system  with  Voltage  Source  Drive,  is  listed  in  Appendix  C.  Both  adaptive 
models  with  servo  motors  are  simulated  at  the  same  tmie  in  the  program  to  assure 
realistic  interaction  between  joint  servo  motors. 

A  sample  region  was  added  to  obtain  the  servo  motor  angular  positions  at 
discrete  time  intervals  PM(N).  The  DELS  parameter  of  the  DSL  VS  simulation 
program  allows  the  simulation  to  enter  the  sample  region  every  T  =  0.25  milliseconds. 
The  adaptive  algorithm,  also  included  in  the  sample  region,  calculates  the  estimates  of 
servo  motor  angular  velocity  (PM)  and  the  second-order  model  gain  parameter  iK^. 
from  the  sample  of  the  servo  motor  angular  position.  At  every  sampling  time,  the 
second-order  model  states  P,  P  are  reset  with  the  values  of  the  servo  motor  position 
and  velocity  PM,  PM  and  the  model  gain  parameter  K^^^  is  updated  during  the  full 
acceleration  part.  Fixed  step  integration  was  used  in  the  simulation  to  support  easy 
hardware  implementation.  The  integration  interval  was  chosen  to  be  1  5  the  sampling 
period. 

B.  SIMULATION  STUDIES  OF  THE  ADAPTIVE  SYSTEM 

Appendix  C  and  D  hst  the  DSL,  VS  simulation  programs  used  in  testing  the 
adaptive  system.  The  simulation  program  for  the  gravity-free  environment  is  included 
in  Appendix  C.   The  simulation  program  in  Appendix  D  includes  gravitational  torques. 

1.  Gravity-free  Environment 

The  adaptive  system  without  gravitational  torques  is  tested  for  both  step 
position  command  and  time  varying  position  command  inputs.  Note  that  on  all  phase 
plane  plots  the  velocity  of  the  second-order  model  and  the  velocity  of  the  servo  motor 
are  both  plotted  as  ordinates.  In  general  these  curves  fall  on  top  of  each  other.  Move 
r?l  and  Move  ^2  are  used  in  this  part  of  the  simulation  studies.  The  following  results 
are  observed: 
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a.   Step  Position  Command  Input  Used 

Figures  6.1  and  6.2  are  the  phase  plane  plot  and  step  response  curves  for 
the  unloaded  arm.  In  the  phase  plane  plot,  the  second-order  angular  velocity  P  and 
the  servo  motor  angular  velocity  PM  are  plotted  versus  servo  motor  angular  position 
PM.  Although  PM  cannot  be  observed  in  the  actual  system,  it  is  available  in  the 
simulation  and  is  used  to  check  the  validity  of  the  algorithm. 

Figure  6.1  shows  that  the  second-order  model  and  servo  motor  have  good 
curve  following  characteristics  for  both  joints.  The  step  response  curves  of  Figure  6.2 
show  the  models  and  servo  motors  tracking  together.  Steady  state  accuracy  of  the 
order  of  IOE-4  is  observed.  The  loaded  arm  was  simulated  in  Figures  6.3  and  6.4  In 
these  figures,  effects  of  the  reaction  torques  can  be  observed.  At  the  beginning  of  the 
move,  upward  motion  of  the  L1NK2  creates  an  interaction  torque  on  JOINTI  servo 
motor.  Therefore  it  takes  time  to  overcome  the  reaction  torque  and  LINKl  starts 
accelerating.  To  avoid  this  unwanted  situation,  the  .position  command  for  J0INT2 
servo  motor  is  delayed  150  millisecond  at  the  beginning  of  the  move.  This  is  shown  in 
Figures  6.5  -  6.8. 

The  effects  of  the  delayed  input  for  phase  plane  trajectories  can  be 
observed  in  Figures  6.5  and  6.7.  When  J0INT2  servo  motor  starts  accelerating,  the 
reaction  torque  causes  JOINTI  servo  motor  to  decelerate  for  a  while.  Meanwhile, 
curve  following  seems  lost  but  after  a  while  the  adaptive  system  tracks  and  follows  the 
curve  until  the  desired  position  is  reached.  Faster  over-all  response  was  obtained  with 
this  strategy  also. 

Simulation  studies  for  Move  U2  are  shown  in  Figures  6.9  -  6.16.  Figures 
6.9  and  6.11  are  good  illustrations  to  point  out  the  importance  of  the  reaction  torques 
between  joints.  Interactive  torques  gain  more  importance  when  servo  motors  operate 
in  opposite  directions.  In  these  figures  it  can  be  observed,  acceleration  of  the  J0INT2 
servo  motor  helps  JOINTI  servo  motor  to  speed  up  quickly.  When  J0INT2  servo 
motor  reaches  the  curve  and  starts  deceleration,  the  interaction  torque  created  by 
J0INT2  servo  motor  opposes  JOINTI  servo  motor  acceleration.  Maximum  angular 
velocity  of  3.7  radsec  and  13.4  rad,  sec  are  observed  for  motors  1  and  2  respectively. 
Step  response  curves  of  Move  #2  are  shown  in  Figures  6.10  and  6.12. 

The  adaptive  system  was  also  simulated  for  a  dilTerent  input  sequence,  ie 
J0INT2  servo  motor  position  command  delayed  at  0.15  second.  The  elTects  of  the 
delayed  input  can  be  observed  in  phase  plane  plots  of  Figures  6.13  and  6.15. 
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Figure  6.S     Step  Response  For  Move  #1  (No  Gravity) 
J0INT2  Servo  \lotor  Input  Delayed  0.15  sec. 
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Figure  6.9     Phase  Plane  Trajectory  For  Move  #2  (No  Gravity). 
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Figure  6.10     Step  Response  For  Move  #2  (No  Gravity). 
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Figure  6.12     Step  Response  For  Move  #2  (No  Gravity). 
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At  the  beginning  of  Move  #2  JOINT  1  servo  motor  moves  slower  while  the 
other  servo  motor  stands  still.  After  J0INT2  servo  motor  starts  accelerating, 
appreciable  speed-up  can  be  observed  in  the  first  servo  motor  phase  plane  trajector}-. 
This  speed-up  results  in  passmg  over  the  curve  a  little.  After  a  while  the  system  tracks 
the  curve  and  follows  it  until  the  position  is  reached.  Step  response  curves  in  Figures 
6.14  and  6.16  show  40  miUiseconds  slower  response  time  in  comparison  to  the 
simulation  results  obtained  when  simultaneous  inputs  are  applied. 

b.    Time  Varying  Position  Command  Input  Used 

To  simulate  the  adaptive  system  under  time  varying  command,  a  ramp 
input  with  tangent  of  1  rad/sec  and  sinusoidal  input  are  used.  Simulation  results  are 
shown  in  Figures  6.1"  -  6. 28.  Phase  plane  trajectories  for  ramp  input  are  shown  in 
Figures  6.17  and  6.20.  Good  curve  following  is  observed  in  the  figures.  J0INT2  servo 
motor  velocity  builds  up  and  becomes  closer  to  steady-state  velocity  in  a  short  time 
interval.  JOINTl  servo  motor  velocity  can't  build  up  as  fast  as  the  second  servo 
motor  because  of  large  inertial  and  interaction  torques.  Therefore  JOINTl  servo 
motor  follows  the  ramp  input  with  larger  errors.  Figures  6.19  and  6.22  show  position 
error  for  servo  motors.  Phase  plane  trajectories  for  sinusoidal  input  are  shown  in 
Figures  6.23  and  6.26.  Good  curve  following  is  seen  in  these  figures.  J0INT2  servo 
motor  follows  the  given  input  command  easily  with  small  errors  but  JOINTl  servo 
motor  lags  behind  the  sine  input  curve  because  of  the  reasons  mentioned  above  in  the 
case  of  a  ramp  input.  Position  error  between  commanded  position  input  and  actual 
servo  motor  position  output  is  given  in  Figures  6.25  and  6.28. 

Simulation  studies  show  that  the  magnitude  of  the  position  error  depends 
on  the  sinusoidal  position  command  input  magnitude  and  frequency.    The  smaller  the 
frequency,  the  smaller  the  position  error  that  is  obtained.    For  a  fixed  frequency,  the 
smaller  magnitude  of  input  gives  a  smaller  position  error. 
2.  Gravitational  Torques  Included 

The  adaptive  system  was  simulated  under  gravitational  torques.  Moves  ^^1.  #2 
and  i?3  were  used  to  test  the  system  for  the  existence  of  excess  interactive  torques. 
Both  step  input  and  time  vari-ing  input  were  used  in  the  simulation  studies. 

a.   Step  Position  Command  Input  is  Used 

Figures  6.29  -  6.32  are  the  phase  plane  plots  and  step  response  curves  for 
simultaneously  applied  step  position  input  commands.  Phase  plane  trajectories  show 
that  JOINTl  servo  motor  develops  negative  velocity  (in  the  opposite  direction  of  the 
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Figure  6.19     Error  Between  Commanded  and  Actual  Position  (No  Gravity). 
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Figure  6.20     Phase  Plane  Trajectory  For  Ramp  Input  (No  Gravity  -  Loaded  Arm). 
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Figure  6.21     Ramp  Response  (No  Gravity  -  Loaded  Arm). 
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Figure  6.22     Error  Between  Commanded  and  Actual  Position 
(No  Gravity  -  Loaded  Arm). 
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Figure  6.24    Sine  Response  (No  Gravity). 
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Figure  6.27     Sine  Response  (No  Gravity  -  Loaded  Arm). 
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Figure  6.29     Phase  Plane  Trajectorv'  For  Move  #1  (With  Gravity). 
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Figure  6.30     Step  Response  For  Move  #1  (With  Gravity). 
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move)  at  the  beginning  of  the  motion.  This  negative  velocity  is  caused  by  gravitational 
torques  and  interactive  torques  in  the  system.  It  can  be  observed  that  gravitational 
torques  are  at  the  maximum  value  when  servo  motors  start  moving.  .After  JOINT  1 
servo  motor  torque  overcomes  the  disturbance  torques,  the  servo  motor  starts 
accelerating  in  the  direction  of  the  move,  catches  and  follows  the  curve  until  the  final 
position  is  reached. 

In  the  case  of  a  loaded  arm,  a  larger  magnitude  of  negative  velocity  is  seen 
because  of  greater  gravitational  torques.  Step  response  curves  are  shown  in  Figures 
6.30  and  6.32.  It  can  be  observed  that  the  negative  velocity  at  the  beginning  of  the 
move  resulted  in  JOINT  1  servo  motor  moving  in  the  wrong  direction  initially.  One 
might  think  that  the  situation  at  the  beginning  of  the  move  may  bring  problems  in 
some  applications.  To  avoid  this  situation.  J01NT2  servo  motor  position  command 
was  delayed  O.l  second  at  the  beginning.  Simulation  results  are  shown  in  Figures  6.33 
-  6.36.  Phase  plane  trajectories  show  that  the  negative  velocity  at  the  beginning  of  the 
move  is  lost.  A  zig-zag  shape  in  JOINTl  servo  motor  phase  trajectorv'  is  seen  due  to 
reaction  torque  produced  by  J0INT2  servo  motor.  The  same  overall  time  response 
was  obtained. 

Simulation  studies  with  Move  #2  are  shown  in  Figures  6.37  -  6.40.  It  can 
be  seen  that  both  second-order  model  and  servo  motor  have  good  curve  following 
capabilities  and  they  are  on  top  of  each  other.  Maximum  velocities  of  6.6  rad,  secand 
22  rad  sec  are  seen  for  servo  motors  1  and  2  respectively.  In  this  sample  move,  the 
reaction  torques  increase  the  servo  motor  acceleration  until  the  curve  is  reached.  After 
that,  reaction  torques  oppose  the  servo  motor  deceleration  along  the  curve.  As  a 
result,  acceleration  time  of  the  move  is  shorter  than  deceleration  time.  Step  response 
curves  are  shown  in  Figures  6.38  and  6.40. 

Move  #3  was  used  for  extensive  testing  of  the  system.  Phase  plane 
trajectories  are  shown  in  Figures  6.41  and  6.43.  It  can  be  seen  at  the  beginning  of  the 
move,  when  servo  motors  start  accelerating,  the  reaction  torque  produced  by  J0INT2 
servo  motor  keeps  JOINTl  servo  motor  from  accelerating,  even  causing  it  to  move  in 
the  opposite  direction  to  the  motion.  This  situation  is  clearer  in  the  step  response 
curves  of  Figures  6.42  and  6.44.  To  avoid  the  unwanted  efTects  of  the  reaction  torques 
at  the  beginning  of  the  motion,  step  position  command  input  to  the  second  servo 
motor  was  delayed  100  milliseconds.  Simulation  results  are  shown  in  Figures  6.45  - 
6.4S.  For  the  unloaded  arm.  a  difierent  input  sequence  worked  out  well.  From  the 
phase  plane  plot  (Figure  6.45)  good  curve  following  can  be  seen. 
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Figure  6.31     Phase  Plane  Trajectory  For  Move  #1  (With  Gravity). 
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Figure  6.32     Step  Response  For  Move  #1  (With  Gravity). 
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Figure  6.33     Phase  Plane  Traiectorv'  For  Move  =^1  (With  Gravity) 
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Figure  6.37     Phase  Plane  Trajectory  For  Move  #2  (With  Gravity). 
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Figure  6. 38     Step  Response  For  Move  #2  (With  Gravity^ 
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Figure  6.41     Phase  Plane  Trajecton-  For  Move  ^3  (With  Gravity). 
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Figure  6.42     Step  Response  For  Move  #3  (With  Gravity). 
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Figure  6.43     Phase  Plane  Trajector}'  For  Move  #3  (With  Gravity). 
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Figure  6.44     Step  Response  For  Move  #3  (With  Gravity). 
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Figure  6.45     Phase  Plane  Traiectorv  For  Move  =;3  (With  Gravitv) 
J0INT2  Servo  Motor  Input  Delayed  0.10  sec. 
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Figure  6.46     Step  Response  For  Move  =:3  (With  Gravitv) 
J0INT2  Servo  Motor  Input  Delayed  0.10  sec. 
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It  is  also  noticed  that  when  J0INT2  servo  motor  starts  accelerating,  the 
reaction  torque  produced  causes  the  JOINT!  sers'o  motor  velocity  to  go  down  for  a 
while,  resulting  in  passing  over  the  curve  and  staying  there  for  a  certain  amount  of 
time  before  it  catches  up  and  follows  the  curve  until  the  desired  position  is  reached. 
From  the  step  response  curve  (Figure  6.46).  60  milliseconds  faster  overall  response  is 
observed. 

Phase  plane  plots  and  step  response  curves  for  Loaded  arm  with  dilTerent 
input  sequences  are  shown  in  Figures  6.47  and  6.48.  Delayed  (100  milliseconds)  step 
position  input  for  J0INT2  servo  motor  causes  JOINTl  servo  motor  to  overshoot. 
This  result  implies  the  importance  of  the  effects  of  the  inertial  torques  and  interaction 
torques  on  the  loaded  arm. 

b.    Time  Varying  Position  Command  Input  Used 

Ramp  input  with  tangent  o^  1  rad  sec  and  a  sinusoidal  input  were  used  to 
test  the  adaptive  system  under  gravitational  torques.  Simulation  results  are  shown  in 
Figures  6.49  -  6.60.  Phase  plane  trajectories  of  Figures  6.49  and  6.52  show  second- 
order  model  and  servo  motor  velocities  are  oscillating  around  the  curve.  From  Figures 
6.50  and  6.53.  it  is  seen  that  J01NT2  servo  motor  follows  the  ramp  input  with  ven.' 
small  errors,  while  JOINT!  servo  motor  is  lagging  behind  the  ramp  input  with  larger 
errors.  Figures  6.51  and  6.54  show  errors  between  commanded  position  input  and 
actual  position  of  the  servo  motors.  It  can  be  observed,  that  the  error  tends  to  go  to 
zero  ver\'  slowly. 

Simulation  studies  with  sinusoidal  input  under  gravitational  torques  are 
shown  in  Figures  6.55  -  6.60.  Good  curve  following  is  observed  from  phase  plane 
trajectories.  Sine  response  curves  in  Figures  6.56  and  6.59,  show  the  second-order 
models  and  servo  motors  follow  the  time  var>'ing  position  command  with  small  errors. 
The  error  between  desired  and  actual  position  is  shown  in  Figures  6.57  and  6.60. 
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J0INT2  Sep,'o  Motor  Input  Delayed  0.10  sec. 
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Figure  6.50     Ramp  Response  (With  Gravity). 
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Figure  6.53     Ramp  Response  (With  Gravity  -  Loaded  Arm). 
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Figure  6.54     Error  Between  Commanded  and  Actual  Position 
(With  Gravity  -  Loaded  Arm). 
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Figure  6.55     Phase  Plane  Trajectorv  For  Sinusoidal  Input 
(With  Gravity). 
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Figure  6.56     Sine  Response  (With  Gravity) 
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Figure  6.59     Sine  Response  (With  Gravity  -  Loaded  Arm). 
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(eqn  7.2} 


VII.  THE  CURRENT  SOURCE  DRIVE  SYSTEM 

A.       DEVELOPMENT  OF  THE  CURRENT  SOURCE  DRIVE  SYSTEM 

In  a  DC  servo  motor;  lo  achieve  constant  acceleration,  the  armature  circuit 
current  must  build  up  quickly  to  a  constant  value  and  provide  constant  torque.  This  is 
important  for  the  second-order  model  to  control  the  servo  motor.  A  current  source 
drive  system  can  be  used  to  accomplish  this. 

It  can  be  shown  [Ref.  2]  that  the  transfer  functions  for  a  permanent  magnet  DC 
servo  motor,  when  the  viscous  friction  of  the  motor  and  load  is  small,  are: 

01  s)  K. 

= (eqn  /.I) 

V{s)       s[(R+sL)Js  +  K^.KJ 

I(s)    _  is 

V{s)        [(R+sL)Js  ^  K^.KJ 

where 

6  =  Angular  position  of  the  shaft 

V  =  Applied  d-c  voltage 

1    =  Armature  current 

K^^  Back  emf  constant 

K._  Torque  constant 

J    =  Total  inertia 

R  =  Armature  resistance 

L  =  Armature  inductance 

The  transfer  function  block  diagram  of  a  Current  Source  Drive  is  shown  [Ref  1]  in 
Figure  7.1. 

If  the  amplifier  gain  is  chosen  as  such  that 

KA  >  >   1  (eqn  7.3) 

then  the  equivalent  transfer  function  of  the  amplifer  can  be  written  as; 
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V       _  [(R^sL)Js  +  K,.KJ 


V, 


Js 


(eqn  7.4) 


The  equivalent  transfer  function  becomes: 


e(s) 

V,(s) 


K^/J 


s- 


(eqn  7.5) 


Thus  if  the  amplifier  gain  is  carefully' chosen,  the  ideal  motor  of  the  second-order 
model  will  be  approximated  by  the  system  and  the  effect  of  the  back  emf  and  armature 
inductance  will  become  negligible. 

Simulation  block,  diagram  of  the  current  source  drive  system  is  shown  in  Figure 
7.2.  Rj  was  chosen  to  be  0.1  to  minmiize  the  influence  on  the  electrical  pole,  inversely 
K^  was  set  at  10.0.  The  amplifier  gam  (KA)  and  saturation  limit  (VS.A.TA)  were 
chosen  by  trial  and  error.   These  values  were  found: 

•  Without  gravitational  toraues 
KA1=  150.  VSATA1=  100 
KA2=75.  VSATA2=50 

•  Gravitational  torques  are  included 
KAl  =  350.  VSATAl  =  300 
KA2  =175.  VSATA2  =   150 


Vq 


VOLTAGE 
AMPLIFIER 
GAIN    =   KA 

V 

"^      1. 

K. 

9 

r^ 

i 

w 

s[(R+sL)Js+K,K^] 

Js 

^ 

[(R-t-sL)Js  + 

KvSC^l 

Fiaure  7.1     Transfer  Function  Block  Diagram  of 
the  Current  Source  Drive. 
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Figure  7.2     Block  Diasram  of  the  Current  Source  Drive  Svstem. 
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B.       SIMULATION  STUDIES  OF  THE  ADAPTIVE  SYSTEM. 

DSL,'VS  simulation  programs  used  in  simulation  of  the  adaptive  system  with 
Current  Source  Drive  are  listed  in  Appendix  E  and  F.  The  simulation  program  listed 
in  Appendix  E  is  used  for  studies  under  a  gravity-free  environment.  Appendix  F  lists 
the  simulation  program  which  includes  gravitational  torques.  Again  note  that  although 
PM  cannot  be  observed  in  the  actual  system,  it  is  available  in  the  simulation  and  is 
used  to  check  the  validity  of  the  algorithm. 

1.  Gravity-Free  Environment 

Step  position  command  and  time  varying  position  command  inputs  are  used 
to  test  the  adaptive  system  under  a  gravity- free  environment  (No  Gravitional  Torques). 
Sample  motions,  Move  ^1  and  Move  U2  are  used  in  the  simulation  studies  of  this  part. 
Note  that  on  all  figures  of  Phase  Plane  Trajectories  the  angular  velocity  of  the  second- 
order  model  (P)  and  the  angular  velocity  of  the  servo  motor  (PM)  are  both  plotted  as 
ordinates.  The  second-order  model  angular  position  (P)  and  the  servo  motor  angular 
position  (PM)  are  both  plotted  as  ordinates  in  the  time  response  plots.  In  general 
these  curves  fall  on  top  of  each  other. 

a.   Step  Position  Command  Input  Used 

Phase  plane  plot  and  step  response  curves  for  the  unloaded  arm  under 
Move  #1  are  shown  in  Figures  7.3  and  7.4.  In  the  phase  plane  plot  PM  and  P  are 
plotted  versus  PM.   The  step  response  curve  shows  P  and  PM  are  plotted  versus  time. 

Good  cur.'e  following  characteristics  are  seen  in  Figure  7.3.  Maximum 
angular  velocity  of  3.17  rad,  sec  and  12  rad/sec  are  observed  for  servo  motors  1  and  2 
respectively.  The  step  response  curves  of  Figure  7.4  show  the  second- order  models  and 
servo  motors  tracking  together.  Also  much  faster  time  response  was  observed  for 
JOINT2  servo  motor  due  to  smaller  inertia  and  supporting  reaction  torques  in  the 
direction  of  motion.    Steady  state  accuracy  of  the  order  of  lOE-4  is  observed. 

■  The  simulation  results  for  the  loaded  arm  are  shown  in  Figures  7.5  and  7.6. 
The  effects  of  the  reaction  torques  can  be  observed  in  these  figures.  At  the  beginning 
of  the  move,  upward  motion  of  the  LINK2  produces  an  interaction  torque  on  JOINTl 
servo  motor.  This  reaction  torque  opposes  JOINT!  servo  motor  acceleration  in  the 
direction  of  motion.  Therefore  it  takes  some  time  to  overcome  the  reaction  torque, 
and  LINKl  starts  accelerating.  To  avoid  this  situation,  J0INT2  servo  motor  position 
command  input  is  delayed  150  milliseconds  at  the  beginning  of  the  Move  ^1. 
Simulation  results  are  shown  in  Figures  7.7  -  7.10. 


103 


ir! 


11- 


3 
§1 


•1. 

-0.1 
51 


9 


51 


-? 


~i        I        I 1 1 1 1 — 

0.1  0.3  0.5  0.7 

Pn2    (RROJ 


•0.1 


— r— 
0.1 


— 1 r 

0.3 


1 — 

0.5 
PM1    fRflOJ 


1 r 


0.7  0.9 


"I r 


15 


-     11 


-       7 


0.9  1.1 


4 


-2!s: 


1.1 


PHRSE  PLfiNE    (CURRENT  SOUR.    DRY. J 


Figure  7.3     Phase  Plane  Trajectory  For  Move  #1  (No  Gravity). 


104 


1.51 


o 
tr. 


a.. 


01 


■0.5_. 


1.5 


1.0 


-       0.5 


CO 


— 1 1 r— 1 1 1 1 1 1 

0.0       0.1       0.2       0.3       0.4       0.5       0.6       0.7       0.8       0.9       1.0 
1  51  ^'"^    (SECOfCIS)  ^^g 


c: 


0.5  to. 


1.1> 


0.5- 


0.0- 


•0.5 


1.0 


-       0.5 


0.0 


— 1 1 1 1 1 1           I  I           I 

0.0       0.1        0.2       0.3       0.4        0.5        O.S       0.7  0.8       0.9       1.0 

Tire    (SECONDS) 

STEP  RESPONSE  (CURRENT  SOUR.  DRV.) 


•0.5  £- 


Figure  7.4    Step  Response  For  Move  #1  (No  Gravity). 


105 


151 


11- 


7- 


•1. 
•0,1 

61 


g 


£1       -2 


T 1 1 1 1 1 ! 1 r 

0.1  0.3  0.5  0.7 

PV2    [RflOl 


IS 


-     11 


'b 


— 1 1 r 

Q.9  1.1 


^ 


4 


UJ 

m 
\ 

Q 
IX 


t  I 1 1 1 1 1 1 r— 1 \ 1 

-0.1  0.1  0.3  0.5  0.7  0.9  1.1 

Pni     (RfiO) 

PHRSE  PLRNE  (CURRENT  SOUR.  DRV.  WITH  LOflO) 


-2E. 


Figure  7.5     Phase  Plane  Trajectory  For  Move  #1  (No  Gravity). 


106 


LSI 


c 


a.1 


x: 
a. 


•0.1 


LCh 


0.5- 


•o.s 


1.5 


1.0 


Q.5 


0.0 


•0.5 


1 1 1 \ 1 1 1 1 1 

0.0       0.1        0.2       0.3       0.4       0.5       0-6       0.7       0.8       0.9       1.0 

1.S1  ^'"^  ^^^^2'  r     1.5 


a 


Q- 


1 1 1 1 1 1 1  I : 

0.0        0.1        0.2       0.3       0.4        0.5       0.5       0.7  0,8       0.9       1.0 

TIME    f SECONDS J 

STEP  RESPONSE  (CURRENT  SOUR.  DRV.  WITH  LOflO) 


1.0 

0.5 

h       0.0 

•0.5  E! 


Figure  7.6     Step  Response  For  Move  #1  (No  Gravity). 


107 


3 


3 


a 


12- 


0- 


•0.1 
SI 


~1 1 1 1 1 1 1 1 1 — 

0.1  0.3  0.5  0.7  0.9 

Ptt2    (RflO) 


16 


-     12 


-       8 


-      4  !H 


1.1 


^ 1 — — I 1 r ! -1 ! 1 1 — — r r 

•0.1  0.1  0.3  0.5  0.7  0.9 

Pni     fRRD) 

PmSE  PLANE    (CURRENT  SOUR.    DRV.  5 


1.1 


Figure  7.7     Phase  Plane  Trajectory"  For  Move  =1  (No  Gravity) 
J0INT2  Servo  Motor  Input  Delayed  0.15  sec. 


108 


a 

c 
ez: 


n 
a. 


LSI 


1.0- 


0.5- 


o.o- 


•0.5. 


T T 


T r 


-I r 


1.5 


1.0 


-       0.5 


0.0 


•0.5 


1.51 


0.0       0.1       0.2       0.3       0.4       0.5       O.S       0.7       0.8       0.9       1.0 

TIME    (SECOTtDSJ 


1.0-* 


0.5- 


•0.5 


1.5 


1.0 


0.5 


-   0.0 


— I 1     I 1 1 1 1 1  I 

0.0   0.1   0.2   0.3   0.4   0.5   0.6   0.7   0.8  0.9   1.0 

Tir£  (SECONDS) 

STEP  RESPONSE  (CURRENT  SOUR,  DRV.) 


■O.S 


Fisure  7.8     Step  Response  For  Move  =1  (No  Graviu-) 
"  J0INT2  Servo  Motor  Input  Delayed  0.15  sec. 


109 


I 


I 


-I 

£1 


161 


1?- 


•0.1 
51 


■H 


16 


-     12 


•t 


"T         r       I         1    — T \ 1 1 1 — I 1 — 

■0.1  0.1  0.3  O.S  0.7  G.9  1.1 

Pfll     (RftOJ 

PHASE  PLANE    (CURRENT  SOUR.    DRY.    U 1 TH  LOAD) 


Figure  7.9     Phase  Plane  Trajectorv  For  Move  =1  (No  Gravitv) 
J0INT2  Servo  Motor  Input  Delayed  0.15  sec. 


110 


g 


a. 


£ 


3Z 
0. 


1.S1 


1.0- 


0.5- 


0.0- 


■O.i 


1.5 


1.0 


0.5 


0.0 


a 


•0.5  tQ_ 


\ 1 1 1 1 \ 1 1 1 

0.0   0.1   0-2   0.3   0.4   0.5   O.S   0.7   0.8   0.9-   1.0 


1.0- 


0.5- 


■0.£ 


1.0 


-   O.S 


-   0.0 


1 1 \ 1 [ 1 1 1 1 

Q.O   0.1   0.2   0.3   0.4   0.5   O.S   0.7   0.8   0.9   1.0 

TIME  (SECONDS) 

STEP  RESPONSE  (CURRENT  SOUR.  DRV.  WITH  LORD) 


•0.5  ta. 
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"  J0INT2  Servo  Motor  Input  Delayed  0.15  sec. 
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The  effects  of  the  delayed  J0INT2  servo  motor  can  be  observed  in  the 
phase  plane  trajectories  of  Figures  7.7  and  7.9.  At  the  beginning,  JOINTl  servo  motor 
starts  accelerating  while  J0INT2  servo  motor  stays  still.  When  J0INT2  servo  motor 
starts  accelerating,  the  reaction  torque  produced  by  J0INT2  servo  miOtor  causes 
JOINTl  servo  motor  to  decelerate  for  a  while.  .VIeanwhile,  curve  following  seems  lost, 
but  after  a  while  the  adaptive  system  tracks  and  follows  the  curve  until  the  fmal 
position.  In  the  loaded  arm  case,  7%  overshoot  was  observed  for  J0INT2  servo 
motor.  This  can  be  prevented  by  lowering  the  curve  gain  constant.  Faster  overall 
response  was  obtained  with  delayed  input. 

Figures  7.11  -  7.18  show  simulation  results  for  Move  #2.  Figures  7.11  and 
7.13  are  good  examples  to  show  the  effects  o^  the  interaction  torques  on  the  servo 
motors.  At  the  beginning  of  the  move,  the  reaction  torques  produced  by  accelerations 
of  the  servo  motors  increase  each  other's  acceleration  in  the  direction  of  the  move. 
Therefore,  servo  motors  reach  maximum  angular  velocities  in  a  short  time.  When 
J0INT2  servo  motor  reaches  the  curve  and  starts  braking,  the  interaction  torque 
produced  by  J0INT2  servo  motor  opposes  JOINTl  servo  motor  acceleration  in  the 
direction  of  the  motion.  Step  response  curves  of  Move  #2  are  shown  in  Figures  7.12 
and  7.14.  It  can  be  observed,  in  the  case  of  loaded  arm,  J0INT2  servo  motor 
overshoots  and  undershoots  before  it  reaches  the  steady-state.  This  is  due  to  the 
reaction  torque  created  by  JOINTl  servo  motor. 

To  take  care  of  this  deficiency,  J0INT2  servo  motor  curve  gain  constant 
was  lowered  to  0.5.  Simulation  results  are  shown  in  Figures  7.15  -  7.18.  Phase  plane 
trajectories  show  that  smaller,  average  velocities  are  obtained  for  JOINT2  servo  motor 
with  a  lowered  deceleration  curve.  Good  curve  following  characteristics  are  observed. 
Step  response  curves  show  almost  no  overshoot  but  longer  time  response  for  J0INT2 
servo  motor.  This  can  be  thought  of  as  a  price  to  be  paid  to  get  no  overshoot.  But  if 
we  think  over-all  time  response  depends  on  JOINTl  servo  motor,  then  this  time 
increase  response  of  J0INT2  servo  motor  doesn't  bring  on  an  additional  time  increase 
to  the  system. 

b.    Time  Varying  Position  Command  Input  Used 

The  adaptive  system  under  time  van."ing  command  input  was  simulated  by 
using  a  ramp  input  with  tangent  of  1  rad,  sec  and  a  sinusoidal  input.  Simulation  results 
are  shown  in  Figures  7.19  -  7.30.  Phase  plane  trajectories  of  Figures  7.19  and  7.22 
show;  J0INT2  servo  motor  velocity  builds  up  quickly  and  reaches  the  steady-state 
velocity  in  a  short  time. 
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Figure  7.14     Step  Response  For  Move  #2  (No  Gravity). 
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Figure  7.15     Phase  Plane  Trajectory  For  Move  #2  (No  Gravity) 
J0INT2  Servo  Motor  Curve  Gain  Constant  Lowered  to  0.5/ 
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Figure  7.16     Step  Response  For  Move  U2  (No  Gravity) 
J0INT2  Servo  Motor  Curve  Gain  Constant  Lowered  to  0.5. 
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Fisure  7.17     Phase  Plane  Trajectony  For  Move  #2  (No  Gravity) 
J0INT2  Servo  Motor  Curve  Gain  Constant  Lowered  to  0.5/ 
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Figure  7.20     Ramp  Response  (No  Gravity). 
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Figure  7.21     Error  Between  Commanded  and  Actual  Position  (No  Gravity). 
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The  second-order  model  and  servo  motor  velocity  of  J0INT2  shudders 
around  the  curve  until  the  desired  position  is  reached.  Large  inertial  torques  and 
interaction  torques  prevent  JOINT!  servo  motor  velocity  to  build  up  as  fast  as 
J0INT2  servo  motor  veloticy.  It  takes  more  time  for  JOINTl  servo  motor  velocity  to 
reach  steady-state  velocity.  Therefore,  JOINTl  servo  motor  follows  the  ramp  input 
with  larger  errors.  Errors  between  commanded  position  input  and  actual  position 
output  of  servo  motor  are  shown  in  Figures  7.21  and  7.24. 

Phase  plane  plots  of  simulation  studies  for  sinusoidal  input  are  shown  in 
Figures  7.25  and  7.28.  Good  curve  following  capabilities  are  seen  in  phase  plane 
trajectories.  From  Figures  7.26  and  7.29.  it  can  be  observed  that  JOINT2  servo  motor 
follows  the  sinusoidal  input  easily  while  JOINTl  servo  motor  lags  behind  the  sine 
input  curve.  The  reasons  for  this  we  mentioned  above  in  the  case  of  a  ramp  input. 
Position  errors  for  servo  motors  are  shown  in  Figures  7.27  and  7.30.  Again  simulation 
studies  show  that  the  smaller  the  frequency  of  the  sinusoidal  position  command  input, 
the  smaller  the  position  error  that  is  obtained.  Also,  for  a  fixed  frequency,  the  smaller 
magnitude  of  input  curve  gives  a  smaller  error. 
2.  Gravitational  Torques  Included 

To  simulate  the  adaptive  system  under  gravitational  torques.  Moves  #1,  #2 
and  #3  were  used.  Again  note  that  on  all  figures  of  phase  plots,  the  second-order 
model  velocity  (P)  and  servo  motor  velocity  (PM)  are  both  plotted  as  ordinates  versus 
servo  motor  angular  position  (PVI).  In  time  response  curves,  the  second-order  model 
angular  position  (PM)  are  both  plotted  as  ordinates.  In  general,  these  curves  fall  on 
top  of  each  other.  In  the  simulation  studies,  both  step  and  time  varying  position 
command  input  were  used. 

a.   Step  Position  Command  Input  Used 

Phase  plane  plots  and  step  response  curves  for  Move  #1  are  shown  in 
Figures  7.31  -  7.34.  In  phase  plane  trajectories  of  Figures  7.31  and  7.33,  it  can  be 
observed  that  gravitational  torques  and  reaction  torque  produced  by  J0INT2  servo 
motor  causes  JOINTl  servo  motor  to  move  in  the  opposite  direction  of  the  move. 
Note  that  gravitational  torques  are  at  the  maximum  value  at  the  beginning  of  the 
move.  It  takes  a  while  to  overcome  the  gravitational  and  reaction  torques,  after  that 
the  JOINTl  servo  motor  stars  accelerating  until  it  catches  and  follows  the  curve  till  the 
desired  position  is  reached. 
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Figure  7.22     Phase  Plane  Trajectory  For  Ramp  Input  (No  Gravity  -  Loaded  Arm). 
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Figure  7.32     Step  Response  For  Move  #1  (With  Gravity). 
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Figure  7.33     Phase  Plane  Trajectory'  For  Move  #1  (With  Gravity). 
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Figure  7.34     Step  Response  For  Move  #1  (With  Gravity). 
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The  phase  plane  plot  for  loaded  arm  {Figure  7.33)  shows  that  the 
magnitude  of  the  negative  velocity  was  increased  for  JOINTl  servo  motor.  When 
J0INT2  servo  motor  catches,  the  curve  starts  deceleration,  JOINT!  servo  motor 
velocity  starts  increasing  verv"  rapidly  until  J0INT2  servo  motor  reaches  the  steady- 
state.  If  we  think  that  the  opposite  movement  of  the  JOINT!  servo  motor  has  to  he 
avoided,  then  a  difTerent  input  sequence  may  be  used  to  accomplish  this.  For  this 
purpose,  J0INT2  servo  position  command  input  was  delayed  200  milliseconds  at  the 
beginning.  Simulation  results  are  shown  in  Figures  7.35  and  7.36.  From  phase  plane 
trajectories,  it  can  be  observed  that  JOINT!  servo  motor  can  start  accelerating  without 
going  in  the  wrong  direction  while  J0INT2  servo  motor  stands  still.  A  zig-zag  shape  is 
seen  in  JOINT!  servo  motor  trajectorv'  due  to  the  reaction  torque  produced  by 
J0INT2  servo  motor  after  it  starts  accelerating.  Also,  from  step  response  (Figure 
7.36),  lO'^.o  overshoot  is  observed  for  J0INT2  servo  motor  due  to  the  reaction  torque 
produced  by  JOINT!  servo  motor.    The  same  overall  time  response  was  obtained. 

Simulation  results  for  Move  ^^2  under  simultaneous  applied  inputs,  are 
shown  in  Figures  7.37  -  7.40.  Phase  plane  trajectories  (Figure  7.37)  show  that  both 
second-order  model  and  servo  motor  have  good  curve  following  capabihties  with 
maximum  velocities  of  6.5  rad/sec  and  27.6  rad/sec  for  the  loaded  arm.  Figure  7.39 
show  that  J0INT2  servo  motor  reaches  steady-state  after  it  overshoots  twice  and 
undershoots  once.  This  is  clearer  in  step  response  curves  of  Figures  7.40.  A  different 
input  sequence  was  used  to  avoid  the  J0INT2  servo  motor  overshoot.  Therefore, 
J0INT2  servo  motor  position  command  input  was  delayed  100  milliseconds.  The 
purpose  of  this  delay  is  to  minimize  the  effect  of  the  reaction  torque  produced  by 
JOINT!  servo  motor  on  J0INT2  servo  motor.  With  delayed  input  J0INT2  servo 
motor  reaches  steady-state  without  overshoot,  while  JOINT!  servo  motor  decelerates. 
This  deceleration  provides  additional  braking  torque  on  the  J0INT2  servo  motor. 
Simulation  results  are  shown  in  Figures  7.4!  -  7.44.  The  same  overall  time  response 
was  obtained. 

Move  #3  was  used  for  additional  testing  of  the  system  under  different 
loading  conditions.  Simulation  results  are  shown  in  Figures  7.45  -  7.48.  Phase  plane 
trajectories  show  that,  at  the  beginning  of  the  move,  the  reaction  torque  produced  by 
acceleration  of  J0INT2  servo  motor  prevents  JOINT!  servo  motor  from  accelerating 
for  a  while.  On  the  contrary'  the  reaction  torques  do  not  work  in  favor  o[  the  servo 
motors  for  Move  ?;3. 
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Figure  7.35     Phase  Plane  Trajectorv  For  Move  ti\  (With  Gravitv) 
J0INT2   Servo  Motor  Input  Delayed  0.20  sec. 
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Figure  7.36     Step  Response  For  Move  #1  (With  Gravitv) 
J0INT2  Servo  Motor  Input  Delaved  0.20  sec. 
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Figure  7.37     Phase  Plane  Trajectory  For  Move  #2  (With  Gravity). 
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Figure  7.38     Step  Response  For  Move  #2  (With  Gravity). 
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Figure  7.41     Phase  Plane  Trajecton'  For  Move  U2  (With  Gravitv) 
J0INT2  Servo  Motor  Input  Delayed  0.10  sec. 
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Figure  7.44     Step  Response  For  VIove  #2  (With  Gravitv) 
J0INT2  Servo  Motor  Input  Delayed  0.10  sec. 
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Figure  7.45     Phase  Plane  Trajecton'  For  Move  #3  (With  Gravity). 
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Figure  7.46     Step  Response  For  Move  #3  (With  Gravity). 
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Figure  7.47     Phase  Plane  Trajectory  For  Move  =3  (With  Gravity). 
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Figure  7.48     Step  Response  For  Move  #3  (With  Gravity). 
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From  phase  plane  trajectories  6.9  rad/sec  and  21.3  rad,  sec,  maximum 
velocities  are  seen  for  servo  motors  1  and  2  respectively.  Good  curve  following 
capabilities  for  both  the  second-order  model  and  servo  motor  are  seen  and  they  are  on 
top  of  each  other  along  the  move. 

b.    Time  Varying  Position  Command  Input  Used 

To  simulate  the  adaptive  system  under  gravitational  torques,  a  ramp  input 
with  a  tangent  of  1  rad,  sec  and  a  sinusoidal  input  were  used.  Simulation  results  are 
shown  in  Figures  7.49  -  7.60. 

Phase  plane  trajectories  of  Figures  7.49  and  7.52  show  that  at  the  beginning 
of  the  move,  the  second-order  model  and  servo  motor  velocities  increase  ven/'  rapidly  to 
reach  the  steady-state  value.  It  can  be  observed  that  this  process  takes  a  much  shorter 
time  for  J0INT2  servo  motor  in  comparison  to  JOINTl  servo  motor.  This  can  be 
thought  to  be  the  result  of  the  reaction  produced  by  J0INT2  servo  motor.  The 
second-order  model  and  servo  motor  angular  velocities  shudder  around  the  curve  to 
follow  it.  Ramp  response  curves  of  Figures  7.50  and  7.53  show  the  J0INT2  servo 
motor  is  able  to  follow  the  ramp  input  easily  with  ver\'  small  errors.  Meanwhile, 
J0INT2  servo  motor  is  lagging  behind  the  ramp  input  because  of  the  reasons 
mentioned  above.  Position  error  between  commanded  and  actual  angular  position  of 
the  servo  motors  are  shown  in  Figures  7.51  and  7.54.  In  the  case  of  a  loaded  arm,  the 
error  reaches  its  maximum  value  at  the  beginning  of  the  move,  after  that  it  decreases 
exponentially. 

Figures  7.55  -  7.60  are  the  simulation  results  obtained  with  sinusoidal 
position  input.  Phase  plane  plots  show  that  good  curve  following  capabilities  are 
observed.  More  shuddering  around  the  curve  is  seen  for  J0INT2  servo  motor.  Sine 
response  curves  of  Figures  7.56  and  7.59  show  that  both  servo  motors  follow  the 
commanded  input.  The  position  error  between  the  desired  angular  position  and  actual 
servo  motor  angular  position  is  shown  in  Figures  7.57  and  7.60. 
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Figure  7.49     Phase  Plane  Trajecton/'  For  Ramp  Input  (With  Gravity). 
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Figure  7.50     Ramp  Response  {With  Gravity). 
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Figure  7.55     Phase  Plane  Trajectorv  For  Sinusoidal  Input 
(With  Gravity). 
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Figure  7.i6     Sine  Response  (With  Gravity). 
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Figure  7.57     Error  Between  Commanded  and  Actual  Position 

(With  Gravity). 
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Figure  7.58     Phase  Plane  Trajecton-'  For  Sinusoidal  Input 
(With  Gravity  -  Loaded  Arm). 
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VIII.  CONCLUSIONS/AREAS  FOR  FURTHER  STUDY 

As  a  result  of  the  simulation  studies  done  in  this  thesis,  the  near  minimum  time 
positioning  of  a  two-link  robot  manipulator  with  an  adaptive  computer  model  seems 
feasible.  Three  sample  moves  were  used  to  remove  as  many  nonlinearities  as  necessary 
to  test  the  adaptive  system  under  large  parameter  variations.  In  these  nonlinearities, 
coupling  inertia,  centripetal  forces,  actuator  dynamics  and  gravitational  torques  were 
included. 

As  observed,  servo  motors  overshoot  for  some  of  the  moves.  These  conditions 
were  overcome  by  offering  alternatives  such  as  lowering  curve  gain  constant  or  using  a 
different  input  sequence. 

In  this  study,  the  direct-drive  arm  was  used  as  a  model.  It  was  observed  that  in 
the  case  of  the  direct  drive  arm,  the  effects  of  the  disturbance  torques  caused  by  the 
coupling  meriia  betv^-een  links,  and  the  centripetal,  coriolis  and  gravitational  forces  on 
■the  robot  arm,  are  larger.  To  overcome  the  disturbance  torques,  the  servo  motor  has 
to  apply  larger  torques.  This  requires  a  servo  motor  with  high  torque  constant  and 
high  armature  current  capabilities.  As  an  alternative  solution  to  this,  a  gear  train  with 
a  gear  ratio  of  N  may  be  used.  Thus  the  effects  q[  the  disturbance  torques  were 
reduced  by  X.  Of  course,  the  price  of  this  alternative  is  a  slower  system.  It  is  up  to 
the  user  to  decide  which  is  the  best  trade-off  for  a  particular  application. 

Simulation  results  show  that  steady-state  accuracy  of  the  order  of  lOE-4  was 
observed  for  the  step  position  command  input.  This  accuracy  may  not  be  enough  for 
some  applications.  Therefore  a  further  study  area  arises  here  to  position  the  LINK2 
tip  more  accurately.    Some  alternative  ways  of  doing  this  may  be  considered  as; 

1.  Replace  the  curve  with  a  linear  cornpensator  and  remove  the  tachometer 
feedback  within  the  adaptive  moderr 

2.  Replace  the  curve  in  the  model  with  a  sain  block  and  modify  the 
tacnometer  feedback  gain. 

In  the  modelling  of  the  arm,  a  rigid  body  was  assumed.    Another  area  for  further 

study  arises  when  a  lightweight,  flexible  structure  is  used  to  build  the  robot  arm.    The 

effects  of  the  mechanical  resonance  is  due  to  the  flexible  structure  on  the  adaptive 

svstem  to  be  studied. 
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In  this  thesis  delayed  steps  were  used  to  prevent  undesired  motions.  To  use  this 
technique  eHectively  in  a  robot,  the  magnitude  of  such  step,  and  the  required  delay 
time  must  be  predetermined  by  on-line  calculation  using  some  sort  of  algorithm  which 
considers  the  initial  oriantations  of  the  arm.  the  commanded  direction  of  the  step.  etc.. 
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APPENDIX  A 
DSL  PROGRAM  FOR  THE  SECOND-ORDER  MODEL  SIMULATION 


TITLE  DSL  PROGRAM  OF  THE  SIMULATION  MODEL 
PARAM  K1=0.60,K2=10000. , KM=4 . 0 , VSAT=150 . ,K=1.0 
PARAM  REF=1.0 
INITIAL 

A=SQRT(2.*KM*VSAT) 
XDOT=0.0 
DYNAMIC 

R=REF'^STEP(0.0) 
E=R-P 

IF ( E . LT . 0 . 0 ) XDOT=- A*K1^S0RT ( ABS ( E ) ) 
IF(E.GE.O.O)XDOT=A'^K1^SQRT(E) 
DERIVATIVE 

aDOTE=XDOT-KPDOT 
KPD0T=PD0T*?; 

V=LIMIT ( -VSAT , VSAT , K2*XD0TE ) 
PDD0T=KM*V 

PD0T= INTGRL (0.0, PDDOT ) 
P=INTGRL(0.0,PDOT) 
TERMINAL 
METHOD  RKSFX 

COMTRL   FINTIM=0 . 15000 , DELT=0  .00005 
PRINT   0.01,P,PDOT,XDOT,V,XDOTE 
SAVE  (G1)0. 00005, P,XD0T,PD0T 
SAVE  (G3)0. 00005, P,REF 

GRAPH(G1/G1,DE=TEK613,P0=1, .5)  P (LE=6 . 5 , UN= ' RAD ' ) 

XDOT(LE=8,NI=8,LO=0. , UN= ' RAD/SEC ' ,SC=$AR) 

PDOT(LE=3,NI=8,LO=0,0,UN=' RAD/SEC'  ,  P0=6  .  5  ,  SC=$AR). 
GRAPH(G3/G3,DE=TEK618,P0=1, .5)  TIME (LE=6 . 5 , UN= ' SECONDS ' ) , . 
P(LE=8,NI=8,LO=0,UN='RAD' ,SC=0.25), . . . 
REF ( LE=3 , NI=3 , L0=0 , SC=0 . 25 , AX=OMIT ) 
'  PHASE  PLANE 
XD0T,PD0T  VS  P 
STEP  RESPONSE 
P  VS  TIME 


LABEL  ( 

;gi 

LABEL  ( 

Gl 

LABEL  ( 

G3 

LABEL  < 

G3 

END 

STOP 
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APPENDIX  B 

DERIVATION  OF  MATHEMATICAL  MODEL  FOR  THE  TWO- 
DEGREES-OF-FREEDOM  PLANAR  ROBOT  ARM 

The  Lagrangian.  L  =  K  —  V  of  the  system  is: 

L  =   1  2{mjdj-  +  J^^j)ej-+l  2(m^d2-  +  Jn^2'^2'^  (eqnB.l) 

+  l/2m2{dj-  +  d2-+2djchcoseo)ej'^ 

+  mo(d-,-  +  djd^cos02)9j0T 

—  [(nij  +  m-))djsin0|  +  mTdTsin(0  j  +  0-,)Jg 

Taking  partial  derivatives  of  Equation  B.l  with  respect  to  0i  and  0-,  : 

aL 


c0i 


=    -  (m| -t-m-))gdicos0j  —  m-)gd')Cos(0i +0-.)  (eqn  B.2) 


=    -  (m-)did7sin0o)0i'' ""{m-)didosin09)0i09  (eqn  B.3) 

"     -m2gd7cos(0^ +  02) 


«  • 


Taking  partial  derivatives  of  Equation  B.l  with  respect  to  0j  and  ©2  : 


—  =  {midj-  +  J^jj0^  (eqn  B.4) 

^        -  m2(dj^  +  d2'^  +  2djd2cos02)0i 


+  m-,(d-)^  +  d,d-,cos0-,)6-, 


2^   2         12 


rg-  =  (m2d2-  +  Jm2')02  +  m2(d22  +  did2COS02)0i  (eqn  B.: 


Taking  derivatives  of  Equations  B.4  and  B.5  with  respect  to  time: 


d    /dl 

dt 


I — :— 1=  (mjdj--(-J     i+m-,dj    +m->d-)''  +  2mTdjd-,cos02)0i  (eqn  B.6) 

^      ^'      +  (m^d2^  +  m2djd2cos02)'02-(2m2djd2sin02)0|02 
—  (m2d|  d')Sin02)0o'' 
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T-f^r  (m,d,-^J^7:)0^4-mo(do^  +  d^d2Cose2)e\  (eqnB.7) 

at     Vet)-./  "       ~  ^     rJ     X       'r^  ^ 

^     -/      -  (mid2d9Sin02)0i07 

Substitude  Equations  B.2  -  B.7  into  Equation  3.1.  obtain: 

T^  =  [(mj  +  m2)d|^  +  mod7^  +  J^|-f2m2djd2COse2]6i  (eqn  B.S) 

+  m2(d7-  + djd2cos62,)0-)  -  (2m7djd2Sin09)0|02 

-  {m-)d|d-,sin0-.)0->-  -H  (nij +m-))gd|Cos0j 
+  m-,gd-,cos(0j  +  6-);) 

T2  =  (m^d2^  +  Jj^^.l0\  +  m^(d2^  +  djd^cos0^)0j  (eqn  B.9) 

—  (m7did-,sin0-)tG|9-,  +  (m-,d,d->sin0-))0.0-) 

-_        ^  ^    ^^  —  - 

+  (m-)djd-.sin0->)9j''  +  m-)gd-,cos{,0j +02) 
When  we  arranse  the  terms  in  Equations  B.S  and  B.9,  we  obtain  Eauation  3.1. 
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APPENDIX  C 

DSL  PROGRAM  FOR  VOLTAGE  SOURCE  DRIVE  (NO 
GRAVITATIONAL  TORQUES) 

TITLE  SIMULATION  PROGRAM  OF  THE  ADAPTIVE  MODEL  (VOLTAGE  SOURCE  DRIVE) 
TITLE  NO  GRAVITATIONAL  TORQUES 

PARAM  K1=0.60,K2=10000. , KM1=0.17 , KM2=4 . 00 , VSAT1=100 . ,K=1 . 0 , T=0. 00025 
PARAM  G=3S6.4,H1=0.243,M2=0.082,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 
PARAM  J1=0.033,J2=0.033,KV1=0.1012,KV2=0.1012,R1=0.91,R2=0.91,K3=0.6 
PARAM  REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , BM1=0 . 0429 , BM2=0 . 0429 , VSAT2=50 . 
INTGER  Nl , N2 , FLAGl , ?LAG2 
INITIAL 

N1=0 

FLAG1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

?1DOT=0. 

P2DOT=0. 

P1DDT=0. 

P2DDT=0. 

?M1=0.0 

?H2=0 . 0 

PH1DT=0. 

PM2DT=0 . 

PM1DDT=0. 

PM2DDT=0 . 

X1DOT=0. 

X2DOT=0. 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

PP1=L/R1 

?P2=L/R2 

TK1=0. 

TM2=0 . 

IMI=0. 

IM2=0 . 

A1=S0RT ( 2 . *KM1*VSAT1 ) 

A2=SdRT  ( 2  .  *KM2'^VSAT2 ) 
DERIVATIVE 

RR1=REF1^STEP(0.0) 

RR2=REF2'^STEP(0.0) 

RRD2=TRANSP (50,0.0,0.10, RR2 ) 

E1=RR1-P1 

E2=RRD2-P2 
NOSORT 

D11  =  (M1+M2)^(D1^'^2)+M2*(D2**2)+2*M2'^D1*D2*C0S(PM2) 

D12=M2'*=(D2^'^2)+M2^Dl^D2^COS(PM2) 

D22=M2*(D2**2) 

D122=-M2^D1^D2^SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PM1+PM2 

*  G1=(M1+H2)=^G*D1*C0S(PM1)+M2'^G*D2*C0S(PMT) 

*  G2=M2^G^D2*C0S(PMT) 

TL1=D12'^PM2DDT+D122*PM2DT^*2  +  2^D112*PM1DT^PM2DT 

TL2=D12^PH1DDT-D211^PH1DT^^2 

JT0T1=J1+D11 
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SORT 


MO SORT 


5 

SORT 


NOSORT 


6 
SORT 


SAMPLE 
NOSORT 


JTOT2=J2+D22 

IF(E1.LT.O.O)X1DOT=-A1'^K1*SQRT(ABS(E1)) 

IF(E1.GE.0.0  X1D0T=A1^K1^S0RT(E1) 

IF  E2.LT.0.0  X2D0T=-A2^K3^SQRT(ABS(E2)) 

IF(E2.GE.0.0)X2DOT=A2'^K3^SQRT(E2) 

X1D0TE=X1D0T-KP1D0T 

KP1D0T=P1D0T^K 

V1=LIMIT(-VSAT1,VSAT1,K2'^X1D0TE) 

IF(FLAG1.EQ.1)G0  TO  5 

IF ( VI. LT.VSATl. AND. TIME. GT.O. 00005 )FLAG1=1 
NSW1=M1 
CONTINUE 

P1DDT=KM1*V1 

P1DOT=INTGRL(0.0,P1DDT) 

P1=INTGRL(0.0,P1DOT) 

VP1=V1-(KV1'^PM1DT) 

IM1=REALPL(0.0,PP1,VP1/R1) 

TM1=KT1^IM1 

TNET1=TM1-PM1DT^BM1-TL1 

PH1DDT=(1./JT0T1)*TNET1 

FniDT=INTGRL(0.0,FMlDDT) 

PH1=INTGRL(0 .0 ,PH1DT) 

X2D0TE=X2D0T-KP2D0T 

KP2DOT=P2DOT^K 

V2=LIMIT ( -VSAT2 , VSAT2 , K2*X2DOTE ) 

IF(FLAG2.EQ.1)G0  TO  6 

IF ( V2 . LT . VSAT2 . AND .TIME . GT . 0 . 0000  5 ) FLAG2=1 
NSW2=N2 
CONTINUE 

P2DDT=KM2*V2 
P2D0T=INTGRL (0.0, P2DDT ) 
P2=INTGRL(0.0,P2DOT) 
VP2=V2-(KV2^PH2DT) 
IM2=REALPL(0.0,PP2,VP2/R2) 
TM2=KT2'^IM2 

TNET2=TM2-PH2DT^BH2-TL2 
PM2DDT= ( 1 . / JT0T2 ) ^TNET2 
PM2DT=INTGRL (0 . 0 , PM2DDT) 
PM2=INTGRL (0.0, PM2DT ) 


21 
20 


IF(N2.EQ.0)GO  TO  21 

IF(Nl.E0.0)GO  TO  20 

P2=PM2 

P1=PM1 

KS2=ABS(2.'^PM2/(V2*((N2*T)**2 

KS1=ABS (2 . ^PMl/ (VI* ( (N1*T) **2 

IF (FLAG2 .EQ . 0 )KM2=KS2 

IF(FLAG1.EQ.0)KM1=KS1 

IF ( N2 .GE . 2 TpM2DTL= (PH2-PM22L) / 

IF(N1.GE.2)PM1DTL=(PH1-PM12L)/ 

IF(FLAG2.E0.0)P2DOT=(2.*((PM2-PH2LSt)/T 

IF  FLAG1.EQ.O)P1DOT=(2.*((PM1-PM1LST)/T 

IF(N2.EQ.NSW2.AND.FLAG2.EQ.1)G0  TO  21 

if(ni.eq.nswi.and.flagi.eo.i)go  to  20 

IF  FLAG2.EQ.1)P2D0T=(2.*(('PM2-PM2LST)/T 

if(flagi.eq.i)pidot=  "  ■  ' 

N2=N2+1 
N1=N1+1 

PM2DTL=P2D0T 

PM1DTL=P1D0T 

P2D0T=PM2DTL 

PM22L=PM2LST 

PM12L=PM1LST 


(PM1-PM1LST)/T 


•PM2DTL 
■PMIDTL 


•PH2DTL 
■PHIDTL 
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SORT 

TERMIN, 

FINISH 

METHOD 

COMTRL 

PRINT 

SAVE 

SAVE 

GRAPH 


GRAPH 


GRAPH 


GRAPH 


*RAPH( 


LABEL 

LABEL 
LABEL 
END 
STOP 


PM2LST=PM2 

?M1LST=PM1 
ER1=ABS(RR1-PM1) 
ER2=ABS(RR2-PH2) 

AL 

PM1=I.00 
RKSFX 
FINTIH=0. 70000, DELT=0. 00005, DELS=. 00025 
0.010, XIDOT , PIDOT , PHIDT , X2D0T , P2D0T , PM2DT , PHI , RRl , PM2 , RR2 , TMl , TM2 
SI)  0.01,  XIDOT, PIDOT, PM1DT,X2D0T,P2D0T,PM2DT,PM1,PM2 
S3)  0.01,  P1,PM1,REF1,P2,PM2,REF2,RR1,RRD2 

(L1/S1,DE=TEK618,P0=1, .5)  PMl (LE=6 . 0 , UN= ' RAD '  ,LO=-0 . 10 , SC=0 . 10 

NI=12) ,?1DOT(LE=4,NI=4,LO=-2.0,UN='RAD/SEC' , SC=2 . 0 , P0=6 . 0) , . . . 
XlDOTaE=4,NI=4,LO=-2.0,SC=2.0,UN='RAD/SEC'  ,AX=OMIT)  ,  .  .  . 
PHIDT ( LE=4 , NI=4 , L0=- 2 . 0 , UM= ' RAD/ SEC ' , SC=2 . 0 ) 

(L2/Sl,DE=TEK613,OV,PO=l,5)  PH2 (LE=6 . 0 ,UN= ' RAD ' ,LO=-0.10 

SC=0.10,NI=12) ,  P2DOT(LE=4,NI=4,LO=0.0,UN=' RAD/SEC  ,SC=5.0, . . . 
P0=6. ) ,X2DOT(LE=4,NI=4,LO=0.0,UN='RAD/SEC' , SC=5 . 0 , AX=0MIT) , . . . 
PM2DT (LE=4 , NI=4 , L0=0 . 0 , UN= ' RAD/SEC ' , SC=5 . 0 ) 
(L3/S3,DE=TEK&18,P0=1, .5)  TIHE (LE=6 . 0 ,UN= ' SECONDS ' ) , . . . 
P1(LE=4,NI=4,L0=-.5,UN='RAD' , SC=0 . 50, P0=6 . 0 ) , . . . 
PH1(LE=4,NI=4,L0=-.5,UH='RAD' ,SC=0.50) , . . . 
RRl ( LE=4 , NI=4 , L0=- . 5 , SC=0 . 50 , AX=OHIT ) 
(L4/S3,DE=TEK613,P0=1,5,0V)  TIHE (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 
P2(LE=4,NI=4,L0=-.5,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . . 
PH2(LE=4,NI=4,L0=-.5,UN='RAD' ,SC=0.50) , . . . 
RRD2 (LE=4 ,NI=4 , L0=- . 5 , SC=0 . 50 , AX=OHIT ) 
L5/S3,DE=TEK613,P0=1, .5)  TIHE (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 

ER1(LE=3,NI=3,LO=0. , UN= ' RAD ' ,SC=0.005) 

ER2 { LE=3 , NI=3 , L0=0 . , UN= ' RAD ' , 5C=0 . 005 , P0=6 . ) 
(LI)  PHASE  PLANE  (VOLTAGE  SOUR.  DRV.  WITH  LOAD) 
(L3)  STEP  RESPONSE (VOLTAGE  SOUR.  DRV.  WITH  LOAD) 
(L5)  ERROR  VS  TIHE  (VOLTAGE  SOUR.  DRV.  WITH  LOAD) 
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APPENDIX  D 

DSL  PROGRAM  FOR  VOLTAGE  SOURCE  DRIVE  (GRAVITATIONAL 

TORQUES  INCLUDED) 

TITLE  SIMULATION  PROGRAM  OF  THE  ADAPTIVE  MODEL  (VOLTAGE  SOURCE  DRIVE) 
TITLE  GRAVITATIONAL  TOROUES  INCLUDED 

PARAM  K1=0 . 60 , K2=10000 . ;KM1=0 .11 , KH2=4 . 00 , VSAT1=300 . , K=l . 0 , T=0 . 00025 
PARAM  G=386.4,M1=0.248,M2=0.032,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 
PARAM  JI=0 . 033 , J2=0 . 033 , KV1=0 . 1012 , KV2=0 . 1012 , R1=0 . 91 , R2=0 . 91 , K3=0 . 6 
PARAM  REF1=1.0,REF2=1.0;L=0. 0001, BM1=0. 0429, BM2=0. 0429, VSAT2=1 50. 
INTGER  N1,N2,FLAG1,FLAG2 
INITIAL 

N1=0 

FLAG1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

P1DOT=0. 

P2DOT=0. 

P1DDT=0. 

P2DDT=0. 

PM1=0.0 

PM2=0 . 0 

?MiDT=0. 

PM2DT=0  . 

PM1DDT=0. 

PM2DDT=0 . 

X1DOT=0. 

X2DOT=0  . 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

PP1=L/R1 

P?2=L/R2 

TM1=0. 

TM2=0. 

IM1=0. 

IM2=0 . 

A1=SQRT ( 2 . *KM1*VSAT1 ) 

A2=SQRT  ( 2  .  *KM2'*'V5  AT2 ) 
DERIVATIVE 

RR1=REF1^STEP(0.0) 

RR2=REF2'^STEP(0.0) 

RRD2=TRANSP ( 50 , 0 . 0 , 0 . 10 , RR2 ) 

E1=RR1-P1 

E2=RRD2-P2 


NOSORT 


D11=(M1+M2)^(D1*'^2)+M2^(D2*^2)+2'*^M2'^D1*D2^C0S(PM2) 

D12=n2^(D2'^^2)+M2^Dl^D2'^C0S(PM2) 

D22=M2*(D2^'^2) 

D122=-M2'^D1'^D2^SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PHT=PH1+PM2 

G1=(M1+M2)^G*D1*C0S(PM1)+M2*G'^D2*C0S(PMT) 

G2=M2^G*D2'^C0S(PHT) 

TL1=D12^PM2DDT+D122*PM2DT^*2  +  2'^D112^PM1DT*PM2DT+G1 

TL2=D12^PM1DDT-D211^PM1DT^'^2+G2 

JT0T1=J1+D11 
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IF( 

;ei, 

.LT, 

.0 

IF( 

El 

.GE, 

.0 

IF( 

E2, 

•  LT, 

.0 

IF( 

!e2, 

.  GE  , 

.0 

SORT 


NOSORT 


JTOT2=J2+D22 

0)X1DOT=-A1*K1*SORT(ABS(E1)) 
0)XlDOT=Ai^Kl^S0RT(El) 
0)X2DOT=-A2'^K3^SORT(ABS(E2)) 
0 )  X2DOT=A2'^K3^SQRT  ( E2 ) 

X1DOTE=X1DOT-KP1DOT 

X?1D0T=P1D0T^K 

V1=LIHIT(-VSAT1,VSAT1,K2^X1D0TE) 

IF(FLAG1.E0.1)GO  TO  5 

IF ( VI . LT . VSATl . AND . TIME . GT . 0 . 0000  5 ) FLAG1=1 


MSW1=N1 
5  CONTINUE 
SORT 


NOSORT 


?1DDT=KI-I1'^V1 

?1DOT=INTGRL(0.0,P1DDT) 

P1=INTGRL(0.0,?1DOT) 

yPl=Vl-(KVl^PMlDT) 

IMi=REALPL(0.0,PPl,VPl/Rl) 

Ti-!1=KT1^IM1 

T:iEri=TMl-PMlDT*BMl-TLl 

?::iDDT=  ( 1 .  /  JTOTl )  ^TNETl 

?::1dt=imtgrl(0  .0  ,pm1ddt) 

?hi=intgrl(o.o,f:iidt) 

x2dote=x2dot-kp2dot 

kp2dot=p2dot'^x 

v2=limit ( -vsat2 , v5at2 , k2^x2dote ) 


SAMPLE 
NOSORT 


IF(FLAG2.E0.1)G0  TO  5 

IF (72 . LT . V5AT2 . AND . TIME . GT . 0 . 00005 ) FLAG2=1 

NSW2=N2 
6  CONTINUE 
SORT 

P2DDT=KM2*V2 

P2D0T=INTGRL (0.0, P2DDT) 

P2=INTGRL(0.0,P2DOT) 

VP2=V2-(KV2'^FM2DT) 

IM2=REAL?L (0.0, PP2 , VP2/R2 ) 

TM2=KT2^IM2 

T!.'ET2=TM2-PM2DT'^3M2-TL2 

?M2DDT=  ( 1  .  /  JT0T2  )  ^TIIET2 

PH2DT=INTGRL (0.0, PM2DDT ) 

PM2=INTGRL (0 . 0 , PM2DT) 

IF(N2.EQ.0)GO  TO  21 
IF(N1.EO.O)GO  TO  20 
?2=?M2 

pl=PHi 

ks2=ABS  ( 2  .  ^PM2/  ( V2^  ( (N2^T)  ^^^2 ) ) ) 

KS1=AB5(2.=^PM1/(V1^((N1^T)^^2))) 

IF(FLAG2.EO.0)KM2=KS2 

IF(FLAG1.E0.0)KM1=KS1 

I F (N2 . GE . 2 ) PM2DTL= ( PM2 - PM2  2L ) / ( 2 . *T ) 

IF (Nl . GE . 2 ) PH1DTL= ( PMl -PMl 2L ) / ( 2 . ^T ) 

I F  FLAG2 . EO . 0 ) P2D0T= ( 2 . ^ ( (PM2-PM2LST ) /T ) ) -PM2DTL 

IF(FLAG1 .EO.O)P1DOT=(2.*( (PH1-PM1LST)/T) )-?MlDTL 

IF(N2.EQ.NSW2.AND.FLAG2.E0.1)GO  TO  21 

IF(N1.E0.NSW1.AMD.FLAG1.E0.1)G0  TO  20 

I F  FLAG2 . EO . i ) P2D0T= ( 2 . * ( ( ?M2 -PM2LST  /T ) ) -PM2DTL 

IF ( FLAGl . EQ . 1 ) ?1D0T= ( 2 . ^ ( ( PHI -PHILST ) /T ) ) -PMIDTL 

21     N2=N2+1 

20     N1=N1+1 

PM2DTL=P2DOT 
PH1DTL=P1D0T 
P2D0T=PH2DTL 
?M22L=PM2LST 
PM12L=PM1LST 
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,LO=-0.10,SC=0.10 
,SC=2.0,PO=6.0) , . 
iX=OMIT) , . . . 


PM2LST=PM2 
PM1LST=PM1 

*  ER1=ABS(RR1-PM1) 

*  ER2=ABS(RR2-PM2) 
SORT 

TERMINAL 

FINISH   PM1=I.00 
METHOD  RKSFX 

CONTRL   FINTIM=0 .70000 , DELT=0 . 00005 , DELS= .00025 

PRINT  0.010, XIDOT , PIDOT , PMIDT , X2D0T , P2D0T , PM2DT , PMl , RRl , PH2 , RR2 , TMl , TM2 
SI)  0.01,  XIDOT, PIDOT, PMIDT, X2D0T,P2D0T,PM2DT, PMl, PM2 
S3)  0.01,  P1,PM1,REF1,P2.PM2,REF2,RR1,RRD2 
(L1/S1,DE=TEK618,P0=1, .5)  PMl (LE=6 . 0 , UN= ' RAD ' 
NI=12) ,P1DOT(LE=4,NI=4,LO=-2.0,UN='RAD/SEC' 
XIDOT (LE=4 , NI=4 , L0=-2 . 0 , SC=2 . 0 , UN= ' RAD/SEC ' ,. 
PMIDT (LE=4 , NI=4 , L0=-2 . 0 , UN= ' RAD/ SEC ' , SC=2 . 0 ) 
(L2/S1,DE=TEK618,0V,P0=1,5)  PM2 (LE=6 . 0 , UN= ' RAD ' ,LO=-0.10, . . . 
SC=0 . 10 , NI=12 ) ,  P2D0T (LE=4 , HI=4 , L0=0 . 0 , UN= ' RAD/ SEC ' , SC=5 . 0 ,' 
P0=6. ) ,X2DOT(LE=4,NI=4,LO=0.0,UN='RAD/SEC' , SC=5 . 0 , AX=0MIT) , 
PM2DT ( LE=4 , NI=4 , L0=0 . 0 , UN= ' RAD/SEC ' , 5C=5 . 0 ) 
(L3/S3,DE=TEK618,P0=1, .5)  TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 
P1(LE=4,NI=4,L0=-.5,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . . 
PM1(LE=4,MI=4,L0=-.5,UN='RAD' ,SC=0.50) , . . . 
RRl ( LE=4 , NI=4 , L0=- . 5 . SC=0 . 50 , AX=0MIT ) 
(L4/S3,DE=TEK613,F0=1,5,0V)  TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 
P2(LE=4,NI=4,L0=-.5,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . . 
PM2(LE=4,NI=4,L0=-.5,UN='RAD' ,SC=0.50) , . . . 
RRD2 ( LE=4 , NI=4 , L0=- . 5 , SC=0 . 50 , AX=OMIT ) 

'^RAPH(L5/S3,DE=TEK&13,PO=l,  .5)  TIME  (LE=6  .  0  ,  UN=  '  SECONDS  '  ) 

■^  ER1(LE  =  8,NI=8,LO=0.  ,  UN=  '  RAD  '  ,SC=0.005)  ,  .  .  . 

ER2 ( LE  =  S , NI  =  8 , L0=0 . , UN= ' RAD '  , SC=0 . 00  5 , P0=6 . ) 


SAVE 
SAVE 
GRAPH 


GRAPH 


GRAPH 


GRAPH 


LABEL 

LABEL 

LABEL 

END 

STOP 


PHASE  PLANE  (VOLTAGE  SOUR.  DRV.  WITH  LOAD) 
STEP  RESPONSE (VOLTAGE  SOUR.  DRV.  VUTH  LOAD) 
ERROR  VS  TIME  (VOLTAGE  SOUR.  DRV.  WITH  LOAD; 
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APPENDIX  E 

DSL  PROGRAM  FOR  CURRENT  SOURCE  DRIVE  (NO 
GRAVITATIONAL  TORQUES) 

TITLE  SIMULATION  PROGRAM  OF  THE  ADAPTIVE  MODEL  (CURRENT  SOURCE  DRIVE) 

TITLE  NO  GRAVITATIONAL  TORQUES 

PARAM  K1=0 . 60 , K2=10000 . , KM1=0 . 17 , KM2=4 . GO , VSAT=100 . , K=l . 0 , T=0 .00025 

PARAH  G=386.4,M1=0.243,H2=0.082,KTI=14.4,KT2=14.4,D1=15. ,D2=10. 

PARAM  J1=0.033,J2=0.033,KV1=0.1012,KV2=0.1012,R1=0.91,R2=0.91,K3=0.6 

PARAM  REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , 5M1=0 . 0429 , BM2=0 . 0429 

PARAM  RS=0.1,VSATA1=100,KA1=150,VSATA2=50,KA2=7  5 

INTGER  Nl , N2 , FLAGl , FLAG2 

INITIAL 

N1=0 

FLAG 1=0 

N2=0 

FLAG2=0 

P1=0.0 

?2=0.0 

P1DOT=0. 

P2DOT=0. 

P1DDT=0. 

P2DDT=0. 

?Mi=0.0 

PM2=0.0 

PM1DT=0. 

PM2DT=0 . 

PM1DDT=0. 

PM2DDT=0 . 

X1D0T=0. 

X2DOT=0. 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

KF=1./RS 

RT0T1=R1+RS 

RT0T2=R2+RS 

PP1=L/RT0T1 

PP2=L/RT0T2 

TM1=0. 

TM2=0. 

IM1=Q. 

IM2=0 . 

A1=S0RT  ( 2  .  '^KMl^VSAT ) 

A2=SQRT  ( 2  .  *KM2'^VSAT ) 
DERIVATIVE 

RR1=REF1*SIN(3 . 14^TIME) 

RR2=REF2^5IN(3.14^TIME) 

*  RRD1=TRA^ISP(50,0.0,0.15,RR1) 

E1=RR1-P1 
E2=RR2-P2 
NOSORT 

D11=(M1+M2)^(D1**2)+M2*(D2**2)+2*M2*D1^D2*C0S(PM2) 

D12=M2^(D2'^*2)+M2^Dl'^D2'^COS(PM2) 

D22=K2^(D2'^'^2) 

D122  =  -M2^D1'^D2*SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PHT=PM1+PM2 

*  G1=(M1+M2)=^G^D1*C0S(PM1)+M2*G'*^D2'^C0S(PMT) 
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G2=M2*G*D2*COS (PMT) 
TL1=D12*PM2DDT+D122*PM2DT*^2+2^D112^PM1DT^PM2DT 
TL2=D12^PM1DDT-D211*PM1DT*^2 
JT0T1=J1+D11 


JT0T2=J2+D22 
IF(E1.LT.0.0 


IF( 

El, 

.GE.O, 

.0 

IF( 

E2, 

.LT.O. 

.0 

IF( 

E2, 

.GE.O. 

.0 

SORT 


NOSORT 


X1D0T=-A1'^K1^SQRT(ABS(E1)  ) 
X1D0T=A1^X1^S0RT(E1) 
X2DOT=-A2'^K3^SQRT(ABS(E2)) 
X2D0T=A2^K3'^SQRT  (E2 ) 


X1D0TE=X1D0T-KP1D0T 

KP1D0T=P1D0T'^K 

V1=LIHIT ( -VSAT , VSAT , K2*X1D0TE) 


5 

SORT 


IF(FLAG1.E0.1)G0  TO  5 
I F ( VI . LT . VSAT . AND .TIME 
NSW1=M1 
CONTINUE 


GT.0.00005)FLAGI=1 


NOSORT 


P1DDT=KM1*V1 

P1DOT=INTGRL(0.0,P1DDT) 

P1=INTGRL(0.0,P1DOT) 

VF1=KF*IM1^RS 

VA1=V1-VF1 

VM1=LIMIT(-VSATA1,VSATA1,VA1*KA1) 

VD1=VH1-KV1'^PM1DT 

IM1=REALPL(0.0,PP1,VD1/RTOT1) 

TH1=KT1^IM1 

TNET1=TH1-PM1DT^BM1-TL1 

PM1DDT= ( 1 . / JTOTl ) ^TNETl 

PH1DT=INTGRL(0.0,?M1DDT) 

PH1=INTGRL(0.0,PM1DT) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT'^X 

V2=LIMIT( -VSAT, VSAT, K2*X2DOTE) 


6 

SORT 


IF (FLAG 2 
IF(V2.LT 
NSW2=N2 
CONTINUE 


,EQ.1)G0  TO  6 
,  VSAT. AND. TIME 


GT. 0.0000 5 )FLAG2=1 


SAMPLE 
NOSORT 


P2DDT=KM2^V2 

P2D0T=INTGRL (0.0, P2DDT) 

P2=INTGRL(0.0,P2DOT) 

VF2=KF^IM2'^RS 

VA2=V2-VF2 

VM2=LIMIT ( -VS ATA2 , VSATA2 , VA2*KA2 ) 

VD2=VM2-KV2^?M2DT 

IH2=REALPL ( 0 . 0 , PP2 , VD2/RT0T2 ) 

TM2=KT2^IM2 

TNET2=TM2-PM2DT*BM2-TL2 

PM2DDT= ( 1 . / JT0T2 ) ^TNET2 

PM2DT=INTGRL (0.0, PM2DDT ) 

?M2= INTGRL (0.0, PH2DT ) 


IF(N2.EO.0)GO  TO  21 

IF(Nl.EQ.0)GO  TO  20 

P2=PM2 

P1=PM1 

KS2=ABS(2.*PM2/(V2*((N2*T)**2 

KS1=ABS(2.^PM1/(V1*((N1*T)^'^2 

IF(FLAG2.E0.0)KM2=KS2 

IF  FLAG1.EQ.0)KM1=KS1 

IF(N2.GE.2)PM2DTL=(PH2-PM22L)/(2.*T) 

IF(N1.GE.2)PM1DTL=(PM1-PM12L)/(2.^T) 

IF  FLAG2.EQ.O)P2DOT=(2.'^((PM2-PM2LST)/T 

IF(FLAG1.EQ.O)P1DOT=(2.^((PH1-PM1LST)/T 

IF(N2.EQ.NSW2.AND.FLAG2.EQ.l)GO  TO  21 


-PM2DTL 
-FMIDTL 
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21 
20 


SORT 

TERMIN 

*INISH 

METHOD 

COMTRL 

PRINT 

SAVE 

SAVE 

GRAPH 


GRAPH 


GRAPH 


GRAPH 


IF(N1.E0.MSW1.AMD.FLAG1.EQ.1)G0  TO  20 

IF(FLAG2.EQ.1)P2D0T=(2.^((PM2-PM2LST)/T))-PM2DTL 

IF(FLAG1.EQ.1)P1D0T=(2.*((PM1-PM1LST)/T))-PM1DTL 
N2=N2+1 
N1=M1+1 

PH2DTL=P2DOT 

PM1DTL=P1D0T 

PH22L=PM2LST 

PM12L=PH1LST 

PM2LST=PM2 

PM1LST=PM1 

ER1=ABS(RR1-PM1) 

ER2=ABS(RR2-PM2) 

L 

PM1=1.0 
RKSFX 
FINTIM=1. 00000, DELT=0. 00005, DELS=0. 000 25 
0.020, XIDOT , PMIDT , PIDOT , X2D0T , P2D0T , PH2DT , PMl , RRl , TLl , PM2 , RR2 , TL2 
SI)  0.001,  XIDOT, PIDOT, PMIDT, X2DOT,P2DOT,PM2DT, PMl, PM2 
S3)  0.001,  P1,FM1,REF1,P2,?M2,REF2,ER1,ER2,RR1,RR2 
(L1/S1,DE=TEK613,P0=1,  .5)  PMl (LE=6 . 0 ,UN= ' RAD '  , L0=-0 . 10 , SC=0 . 1 ,  .  .  . 
NI=12) ,PlD0T(LE=4,MI=6,L0=-6. , UN= ' RAD/SEC ' , SC=3 . 0 , P0=6 . 0 ) , . . . 
XIDOT (LE=4,HI=6,SC=3.0,LO=-&. , UM= ' RAD/SEC ' ,AX=OMIT) , . . . 
PM1DT(LE=4 , NI=6 , L0=-6 . , UN= ' RAD/SEC ' , SC=3 . 0 ) 
(L2/Sl,DE=TEK618,FO=l,5,OV)  PM2 (LE=5 . 0 , UN= ' RAD ' ,LO=-0.1, . . . 

SC=0.10,NI=12) ,  P2DOT(LE=4,NI=4,LO=-6.0,UM=' RAD/SEC  ,SC=3.0 

P0=6.0) ,  X2DOT(LE=4,NI=4,LO=-6.0,UM='RAD/SEC' , SC=3 . 0 , AX=OMIT) , . . 
PH2DT (LE=4 , NI=4 , L0=-6 . 0 , UM= ' RAD/SEC ' , SC=3 . 0 ) 
(L3/S3,DE=TEK518,P0=1, .5)  TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 
P1(LE=4,MI=3,LO=0.0,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . . 
PM1(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0.50) , . . . 
RRl (LE=4 , NI=3 , L0=0 . 0 , SC=0 . 50 , AX=OMIT ) 
(L4/S3 .DE=TEK618,PO=l,5,OV)  TIME (LE=6 . 0 , UN= ' SECONDS ' ) , . . . 
P2 (LE=4 , NI=3 , L0=0 . 0 , UN= ' RAD ' , SC=0 . 5 , P0=6 . 0 ) , . . . 
PM2(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0.5) , . . . 

0,SC=0.5,Aa=OMIT) 
5 )  TIME ( LE=6 . 0 , UN= ' SECONDS '),... 
,UN='RAD' ,5C=0.05) , . . . 
,UN='RAD' ,SC=0.05,PO=6. ) 
LI)  PHASE  PLANE  (CURRENT  SOUR.  DRV.  WITH  LOAD) 
L3)  SINE  RESPONSE  (CURRENT  SOUR.  DRV.  WITH  LOAD) 
L5)  ERROR  VS  TIME  (CURRENT  SOUR.  DRV.  WITH  LOAD) 


RR2(LE=4,NI=3,LO=0 

GRAPH  (L5/S3,DE=TEK618,PO=l, 
ER1(LE=8,NI=8,LO=0 
ER2(LE=3,NI=3,LO=0 

LABEL 

LABEL 

LABEL 

END 

STOP 
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APPENDIX  F 

DSL  PROGRAM  FOR  CURRENT  SOURCE  DRIVE  (GRAVITATIONAL 

TORQUES  INCLUDED) 

TITLE  SIMULATION  PROGRAM  OF  THE  ADAPTIVE  MODEL  (CURRENT  SOURCE  DRIVE) 

TITLE  GRAVITATIONAL  TORQUES  INCLUDED 

PARAM  K1=0.60,K2=10000. , KM1=0 . 17 , KM2=4 . 00 , VSAT=300 . ,K=1 . 0 , T=0 . 00025 

PARAM  G=386.4,M1=0.248,M2=0.082,KT1=14.4,KT2=14.4,D1=15. ,D2=10. 

PARAM  J1=0.033,J2=0.033,KV1=0.1012,KV2=0.1012,R1=0.91,R2=0.91,K3=0.6 

PARAM  REF1=1 . 0 , REF2=1 . 0 , L=0 . 0001 , BH1=0 . 0429 , BM2=0 . 0429 

PARAM  RS=0 . 1 , VSATA1=300 ,KA1=350 , V5ATA2=150 ,KA2=175 

INTGER  N1,N2,FLAG1,FLAG2 

INITIAL 

N1=0 

FLAG1=0 

N2=0 

FLAG2=0 

P1=0.0 

P2=0.0 

P1DOT=0. 

P2DOT=0. 

P1DDT=0. 

P2DDT=0. 

PM1=0.0 

PM2=0  . 0 

PM1DT=0. 

PM2DT=0 . 

PM1DDT=0. 

PM2DDT=0 . 

X1DOT=0. 

X2DOT=0. 

TL1=0. 

TL2=0. 

TL11=0. 

TL22=0. 

KF=1./RS 

RT0T1=R1+RS 

RT0T2=R2+RS 

PP1=L/RT0T1 

PP2=L/RT0T2 

TM1=0. 

TM2=0. 

IM1=0. 

IM2=0. 

A1=S0RT(2.*KM1'^VSAT) 

A2=SQRT ( 2 . *KM2*VSAT ) 
DERIVATIVE 

RR1=REF1'^SIM ( 3  .  14*TIME ) 

RR2=REF2'^SIN(3.14*TIME) 
*      RRD1=TRANSP(50,0.0,0.15,RR1)  ' 

E1=RR1-P1 

E2=RR2-P2 


NOSORT 


D11=(M1+M2)^(D1*'^2)+M2*(D2**2)+2'^M2*D1*D2'^C0S(PM2) 

D12=M2*(D2'^'^2)+M2*D1^D2'^C0S(PM2) 

D22=M2*(D2*^2) 

D122=-M2^D1^D2*SIN(PM2) 

D211=D122 

D112=D211 

D121=D112 

D212=D121 

D221=D212 

PMT=PM1+PM2 

Gl=  (M1+M2 ) '^G'^Dl'^COS  ( PMl ) +M2*G*D2*C0S  ( PMT ) 


ISO 


SORT 


NOSORT 


G2=M2'^G'^D2*COS  (PMT) 

TL1=D12^PH2DDT+D122*PM2DT*^2+2^D112*PM1DT*PM2DT+G1 

TL2=D12^PH1DDT-D211^PM1DT'^'^2+G2 

JT0T1=J1+D11 

JTOT2=J2+D22 

IF (E1,LT.O.O)X1DOT=-A1*K1^SORT(ABS(E1)) 

IF(E1.GE.O.O)X1DOT=A1'^K1^SORT(E1) 

IF(E2.LT.O.O)X2DOT=-A2^K3^SORT(ABS(E2)) 

IF(E2.GE.0.0)X2DOT=A2^K3^SQRT(E2) 

X1D0TE=X1D0T-XP1D0T 

KP1D0T=P1D0T^K 

V1=LIMIT(-VSAT,VSAT,K2*X1D0TE) 


IF(FLAG1.E0.1)G0  TO  5 

IF ( VI . LT .VSAT .AND .TIME .GT . 0 . 00005 ) FLAG1=1 
NSW1=M1 
5     CONTINUE 


SORT 


NOSORT 


P1DDT=KM1*V1 

P1D0T=INTGRL (0 . 0 , PIDDT) 

P1=INTGRL(0.0,P1DOT) 

VF1=KF^IM1*RS 

VA1=V1-VF1 

VM1=LIMIT(-VSATA1,VSATA1,VA1'*'KA1) 

VD1=VH1-KV1^PM1DT 

IM1=REALPL(0.0,PP1,VD1/RTOT1) 

TM1=KT1'^IM1 

TNET1=TM1-PM1DT'^BM1-TL1 

PM1DDT=(1 ./JT0T1)^TMET1 

PM1DT=INTGRL(0.0,PM1DDT) 

PM1=INTGRL (0.0, PMIDT ) 

X2DOTE=X2DOT-KP2DOT 

KP2DOT=P2DOT^K 

V2=LIMIT ( -VSAT , VSAT , K2*X2D0TE ) 

IF(FLAG2.E0.1)G0  TO  6 

IF ( V2 . LT .VSAT .AND .TIME . GT . 0 . 00005 ) FLAG2=1 


NSW2=N2 
6  CONTINUE 
SORT 


SAMPLE 
NOSORT 


P2DDT=KM2^V2 

P2D0T=INTGRL (0.0, P2DDT) 

P2=INTGRL(0.0,P2DOT) 

VF2=KF^IM2'^RS 

VA2=V2-VF2 

VM2=LIMIT(-VSATA2,VSATA2,VA2'*'KA2) 

VD2=VM2-KV2^?M2DT 

IM2=REAL?L (0.0, PP2 , VD2/RTOT2 ) 

TM2=KT2^IM2 

TNET2=TM2-PM2DT*BM2-TL2 

PM2DDT= ( 1 . / JT0T2 ) *TNET2 

PM2DT=INTGRL ( 0 . 0 , PH2DDT ) 

PM2=INTGRL (0.0, PM2DT ) 


IF(N2.EQ.0)GO  TO  21 

IF(N1.EO.O)GO  TO  20 

P2=PM2 

P1=PM1 

K52=ABS(2.*PM2/(V2*((N2*T)*^2' 

KS1=ABS(2.*FM1/(V1*((N1^T)*^2' 

I F ( FLAG2 . EQ . 0 ) KM2=KS2 

IF(FLAG1.EQ.0)KM1=KS1 

IF  N2 . GE . 2 )PM2DTL= (PM2-PM22L) / (2 . *t; 

IF(N1.GE.2)PM1DTL=(PM1-PM12L)/(2.^T, 

IF  ( FLAG2  .  EO  .  0  )  ?2D0T=  ( 2  .  =^  (  ( PH2 -PH2LST )  /T ) )  -PM2DTL 

IFfFLAGl  .EQ.O)P1DOT=(2.'^(  (PH1-PH1LST)/T)  )-PMlDTL 

IF(N2.EQ.NSW2.AND.FLAG2.EQ.l)GO  TO  21 
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IF(N1.E0.MSW1.AND.FLAG1.EQ.1)G0  TO  20 
IF ( FLAG2 . EQ . 1 ) P2D0T= ( 2 . ^ ( (PM2-PM2LST ) /T ) ) -PM2DTL 
IF ( FLAGl . EQ . 1 ) P1D0T= ( 2  > ( (PMI-PHILST ) /T ) ) -PMIDTL 
21    N2=N2+1 
20    N1=M1+1  . 

PM2DTL=P2DOT 
PM1DTL=P1D0T 
PM22L=PM2LST 
PM12L=PM1LST 
PM2LST=PM2 
PM1LST=FM1 
ER1=ABS(RR1-PM1) 
ER2=ABS(RR2-PM2) 
SORT 

TERMINAL 
^INISH   PM1=1.0 
METHOD  RKSFX 

CONTRL   FIMTIM=1. 00000, DELT=0. 00005, DELS=0. 00025 

PRINT  0.020, XIDOT , PMIDT , PIDOT , X2D0T , P2D0T , PM2DT , PMl , RRl , TLl , PM2, RR2 , TL2 
SI)  0.001,  XIDOT, PIDOT, PMIDT, X2D0T,P2D0T,PM2DT, PMl, PM2 
■S3)  0.001,  P1,PM1,REF1,P2,PM2,REF2,ER1,ER2,RR1,RR2 

^'  PM1(LE=6.0,UN='RAD' ,LO=-0.1,SC=0.1, . . . 

,L0=-o. ,UN=' RAD/SEC  , SC=3 . 0 ,P0=6 . 0 ) 

,L0=-6. ,UH=' RAD/SEC ,AX=OMIT) , . . . 
,UN='RAD/SEC' ,SC=3.0) 

(L2/Sl,DE=TEKol3,PO=l,5,OV)  PM2 (LE=6 . 0 , UN= ' RAD ' ,LO=-0.1 

SC=0.10,NI=12) ,  P2DOT(LE=4,HI=4,LO=-6.0,UN='RAD/SEC' ,SC=3.0, . . . 


SAVE 
SAVE 
GRAPH 


GRAPH 


(Ll/Sl ,DE=TEK618,P0=1 , .5) 
NI=12) ,P1D0T(LE=4,NI=6 
XIDOT (LE=4,HI=6,SC=3.0 
PMIDT (LE=4,MI=6,L0=-6. 


P0=6.0) ,  X2DOT(LE=4,NI=4,LO=-6.0,UN='RAD/SEC 

PM2DT (LE=4 , HI=4 , L0=-6 . 0 , UN= ' RAD/SEC ' , SC=3 . 0 ) 

GRAPH  (L3/S3,DE=TEKS18,P0=1, .5)  TIME (LE=6 . 0 , UN= ' SECONDS 

?1(LE=4,NI=3,LO=0.0,UN='RAD' , SC=0 . 50 , P0=6 . 0 ) , . . 

FM1(LE=4,NI=3,LO=0.0,UN='RAD' ,SC=0.50) 

RRl (LE=4 ,NI=3 ,LO=0 .0 ,SC=0 . 50 , AX=OMIT) 
L4/S3 . DE-TEK618 , P0=1 , 5 , OV )  TIME ( LE=6 . 0 , UH= ' 
P2 (LE=4 , NI=3 , L0=0 . 0 , UM= ' RAD ' , SC=0 . 5 , P0=6 
PM2(LE=4,NI  =  3,LO=0.0,UN='RAD'  ,SC=0.5) ,  .  . 

0,SC=0.5,AX=OMIT) 
5)  TIME (LE=6.0,UN=' SECONDS 

,UN='RAD' ,SC=0.05) 

UN='RAD' ,SC=0.05,PO=6. ) 
PHASE  PLANE  (CURRENT  SOUR.  DRV.  WITH  LOAD) 
SINE  RESPONSE  (CURRENT  SOUR.  DRV.  WITH  LOAD) 
ERROR  VS  TIME   CURRENT  SOUR.  DRV.  WITH  LOAD) 


SC=3. ,AX=OMIT 


GRAPH 


GRAPH 


) 


SECONDS 
.0),... 


LABEL 

LABEL 

LABEL 

END 

STOP 


RR2(LE=4,NI=3,LO=0. 

(L5/S3,DE=TEK618,PO=l, . 

ER1(LE=8,NI=8,LO=0. 

ER2(LE=8,NI=8,LO=0. 
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